#!/usr/bin/env python

import time
import math
import numpy as np
from numpy.linalg import norm
from sklearn.metrics import mean_squared_error
from scipy.spatial import Voronoi, voronoi_plot_2d

import igraph
from bresenham import bresenham

import copy
import matplotlib

matplotlib.use('Agg')
import matplotlib.pyplot as plt

import rospy
from threading import Lock
from nav_msgs.msg import OccupancyGrid
from geometry_msgs.msg import Pose, PointStamped
from gvgexploration.msg import *
from gvgexploration.srv import *
import project_utils as pu
from project_utils import euclidean_distance
import tf
from graham_scan import graham_scan
import shapely.geometry as sg
from nav_msgs.msg import *
from nav2d_navigator.msg import *
from std_srvs.srv import *
from project_utils import INDEX_FOR_X, INDEX_FOR_Y, save_data
from std_msgs.msg import String

import shapely.geometry as sg
from shapely.geometry.polygon import Polygon
from visualization_msgs.msg import Marker
from tf import TransformListener
from geometry_msgs.msg import Point
from scipy.ndimage import minimum_filter

from nav_msgs.srv import GetMap

INF = 100000
SCALE = 10
FREE = 0.0
OCCUPIED = 90.0
UNKNOWN = -1.0
SMALL = 0.000000000001
FONT_SIZE = 16
MARKER_SIZE = 12

class Grid:
    """Occupancy Grid."""

    def __init__(self, map_msg):
        self.header = map_msg.header
        self.origin_translation = [map_msg.info.origin.position.x,
            map_msg.info.origin.position.y, map_msg.info.origin.position.z]
        self.origin_quaternion = [map_msg.info.origin.orientation.x,
            map_msg.info.origin.orientation.y,
            map_msg.info.origin.orientation.z,
            map_msg.info.origin.orientation.w]
        self.grid = np.reshape(map_msg.data,
            (map_msg.info.height,
                map_msg.info.width)) #shape: 0: height, 1: width.
        self.resolution = map_msg.info.resolution # cell size in meters.

        self.tf_listener = TransformListener() # Transformation listener.

        self.transformation_matrix_map_grid = self.tf_listener.fromTranslationRotation(
            self.origin_translation,
            self.origin_quaternion)

        self.transformation_matrix_grid_map = np.linalg.inv(self.transformation_matrix_map_grid)

        # Array to check neighbors.
        self.cell_radius = int(10 / self.resolution) # TODO parameter
        self.footprint = np.ones((self.cell_radius+1,self.cell_radius+1))
        #self.plot()

    def plot(self):
        fig1 = plt.gcf()
        plt.imshow(self.grid)
        plt.colorbar()
        plt.gca().invert_yaxis()
        plt.xlabel('xlabel', fontsize=18)
        plt.ylabel('ylabel', fontsize=16)
        fig1.savefig('map.png', dpi=100)

    def cell_at(self, x, y):
        """Return cell value at x (column), y (row)."""
        return self.grid[int(y), int(x)]
        #return self.grid[x + y * self.width]

    def is_free(self, x, y):
        if self.within_boundaries(x, y):
            return 0 <= self.cell_at(x, y) < 50 # TODO: set values.
        else:
            return False

    def is_obstacle(self, x, y):
        if self.within_boundaries(x, y):
            return self.cell_at(x, y) >= 50 # TODO: set values.
        else:
            return False

    def is_unknown(self, x, y):
        if self.within_boundaries(x, y):
            return self.cell_at(x, y) < 0 # TODO: set values.
        else:
            return True

    def within_boundaries(self, x, y):
        if 0 <= y < self.grid.shape[0] and 0 <= x < self.grid.shape[1]:
            return True
        else:
            return False

    def convert_coordinates_i_to_xy(self, i):
        """Convert coordinates if the index is given on the flattened array."""
        x = i % self.grid.shape[1] # col
        y = i / self.grid.shape[1] # row
        return x, y

    def wall_cells(self):
        """
        Return only *wall cells* -- i.e. obstacle cells that have free or
            unknown cells as neighbors -- as columns and rows.
        """

        # Array to check neighbors.
        window = np.asarray([
            [1, 1, 1],
            [1, 0, 1],
            [1, 1, 1]
        ])
        # Return all cells that are obstacles and satisfy the neighbor_condition
        neighbor_condition = self.grid > minimum_filter(self.grid,
                footprint=window, mode='constant', cval=10000) # TODO: value.
        obstacles = np.nonzero((self.grid >= OCCUPIED) & neighbor_condition)
        obstacles = np.stack((obstacles[1], obstacles[0]),
            axis=1) # 1st column x, 2nd column, y.

        return obstacles

    def is_frontier(self, previous_cell, current_cell,
        distance):
        """current_cell a frontier?"""
        v = previous_cell - current_cell
        u = v/np.linalg.norm(v)

        end_cell = current_cell - distance * u

        x1, y1 = current_cell.astype(int)
        x2, y2 = end_cell.astype(int)

        for p in list(bresenham(x1, y1, x2, y2)):
            if self.is_unknown(*p):
                return True
            elif self.is_obstacle(*p):
                return False
        return False

    def line_in_unknown(self, start_cell, end_cell):
        x1, y1 = start_cell.astype(int)
        x2, y2 = end_cell.astype(int)

        unknown_cells_counter = 0
        for p in list(bresenham(x1, y1, x2, y2)):
            if self.is_unknown(*p):
                unknown_cells_counter += 1
            elif self.is_obstacle(*p):
                return False
        return unknown_cells_counter

    def unknown_area_approximate(self, cell):
        """Approximate unknown area with the robot at cell."""
        cell_x = int(cell[INDEX_FOR_X])
        cell_y = int(cell[INDEX_FOR_Y])

        min_x = np.max((0, cell_x - self.cell_radius))
        max_x = np.min((self.grid.shape[1], cell_x + self.cell_radius + 1))
        min_y = np.max((0, cell_y - self.cell_radius))
        max_y = np.min((self.grid.shape[0], cell_y + self.cell_radius + 1))

        return (self.grid[min_y:max_y, min_x:max_x] < FREE).sum()

    def unknown_area(self, cell): # TODO orientation of the robot if fov is not 360 degrees
        """Return unknown area with the robot at cell"""
        unknown_cells = set()

        shadow_angle = set()
        cell_x = int(cell[INDEX_FOR_X])
        cell_y = int(cell[INDEX_FOR_Y])
        for d in np.arange(1, self.cell_radius): # TODO orientation
            for x in range(cell_x-d, cell_x+d+1): # go over x axis
                for y in range(cell_y-d,cell_y+d+1): # go over y axis
                    if self.within_boundaries(x, y):
                        angle = np.around(np.rad2deg(pu.theta(cell, [x,y])),decimals=1) # TODO parameter
                        if angle not in shadow_angle:
                            if self.is_obstacle(x, y):
                                shadow_angle.add(angle)
                            elif self.is_unknown(x, y):
                                unknown_cells.add((x,y))

        return len(unknown_cells)

    def pose_to_grid(self, pose):
        """Pose (x,y) in header.frame_id to grid coordinates"""
        # Get transformation matrix map-occupancy grid.
        return (self.transformation_matrix_grid_map.dot([pose[0], pose[1], 0, 1]))[0:2] / self.resolution # TODO check int.

    def grid_to_pose(self, grid_coordinate):
        """Pose (x,y) in grid coordinates to pose in frame_id"""
        # Get transformation matrix map-occupancy grid.
        return (self.transformation_matrix_map_grid.dot(
                    np.array([grid_coordinate[0]* self.resolution,
                    grid_coordinate[1]* self.resolution, 0, 1])))[0:2]

    def get_explored_region(self):
        """ Get all the explored cells on the grid map"""

        # TODO refactor the code, right now hardcoded to quickly solve the problem of non-common areas.
        # TODO more in general use matrices.
        def nearest_multiple(number, res=0.2):
            return np.round(res*np.floor(round(number / res, 2)),1)

        p_in_sender = PointStamped()
        p_in_sender.header = self.header

        poses=set()
        self.tf_listener.waitForTransform("robot_0/map",
                                   self.header.frame_id, rospy.Time(),
                                   rospy.Duration(4.0))
        p_in_sender.header.stamp = rospy.Time()
        for x in range(self.grid.shape[1]):
            for y in range(self.grid.shape[0]):
                if self.is_free(x,y):
                    p=self.grid_to_pose((x,y))
                    p_in_sender.point.x = p[0]
                    p_in_sender.point.y = p[1]

                    p_in_common_ref_frame = self.tf_listener.transformPoint("robot_0/map", p_in_sender).point
                    poses.add((nearest_multiple(p_in_common_ref_frame.x), nearest_multiple(p_in_common_ref_frame.y)))
        return poses

class Graph:
    def __init__(self):
        # Robot ID.
        self.robot_id = rospy.get_param("~robot_id")

        # Parameters related to the experiment.
        self.robot_count = rospy.get_param("/robot_count")
        self.environment = rospy.get_param("/environment")
        self.run = rospy.get_param("/run")
        self.bs_pose = rospy.get_param("/bs_pose")
        self.img_res = rospy.get_param("/image_resolution")

        # Physical parameters of the robot.
        self.robot_radius = rospy.get_param('/robot_{}/robot_radius'.format(self.robot_id)) # meter.

        # Internal data.
        self.latest_map = None # Grid map.

        self.tf_listener = TransformListener() # Transformation listener.

        # ROS publisher
        self.marker_pub = rospy.Publisher('voronoi', Marker, queue_size=0) # log.

        # TO CHECK WHAT IS NEEDED.
        self.min_hallway_width = None
        self.pixel_desc = {}
        self.all_poses = set()
        self.obstacles = {}
        self.free_points = {}
        self.map_resolution = 0.05
        self.plot_intersection_active = False
        self.plot_data_active = False
        self.lock = Lock()

        self.obstacles = {}
        self.adj_list = {}
        self.edges = {}
        self.leaf_slope = {}
        self.longest = None
        self.adj_dict = {}
        self.tree_size = {}
        self.leave_dict = {}
        self.parent_dict = {}
        self.new_information = {}
        self.known_points = {}
        self.unknown_points = {}
        self.global_leaves = {}
        self.leaf_edges = {}
        self.leaf_obstacles = {}
        self.performance_data = []
        self.explored_points = set()
        self.last_intersection = None

        self.prev_ridge = None

        self.debug_mode = rospy.get_param("/debug_mode")
        self.method = rospy.get_param("/method")
        self.bs_pose = rospy.get_param('/bs_pose')
        self.map_scale = rospy.get_param('/map_scale')
        self.graph_scale = rospy.get_param("/graph_scale")
        self.termination_metric = rospy.get_param("/termination_metric")
        self.max_target_info_ratio=rospy.get_param("/max_target_info_ratio")
        self.frontier_threshold = rospy.get_param("/frontier_threshold")
        self.min_hallway_width = rospy.get_param("/min_hallway_width") * self.graph_scale
        self.comm_range = rospy.get_param("/comm_range") * self.graph_scale
        self.point_precision = rospy.get_param("/point_precision")

        self.lidar_scan_radius = rospy.get_param("/lidar_scan_radius") * self.graph_scale
        self.lidar_fov = rospy.get_param("/lidar_fov")
        self.slope_bias = rospy.get_param("/slope_bias")
        self.separation_bias = rospy.get_param("/separation_bias") * self.graph_scale
        self.opposite_vector_bias = rospy.get_param("/opposite_vector_bias")
        rospy.Service('/robot_{}/rendezvous'.format(self.robot_id), RendezvousPoints,self.fetch_rendezvous_points_handler)
        rospy.Service('/robot_{}/explored_region'.format(self.robot_id), ExploredRegion,self.fetch_explored_region_handler)
        rospy.Service('/robot_{}/frontier_points'.format(self.robot_id), FrontierPoint, self.frontier_point_handler)
        rospy.Service('/robot_{}/check_intersections'.format(self.robot_id), Intersections, self.intersection_handler)
        rospy.Service('/robot_{}/fetch_graph'.format(self.robot_id), FetchGraph, self.fetch_edge_handler)
        # rospy.Subscriber('/robot_{}/map'.format(self.robot_id),OccupancyGrid, self.map_callback)
        rospy.Subscriber('/shutdown', String, self.save_all_data)

        rospy.wait_for_service('/robot_{}/static_map'.format(self.robot_id))
        self.get_map = rospy.ServiceProxy('/robot_{}/static_map'.format(self.robot_id), GetMap)

        self.already_shutdown = False
        self.robot_pose = None

        # edges
        self.deleted_nodes = {}
        self.deleted_obstacles = {}
        self.last_graph_update_time = rospy.Time.now().to_sec()
        # rospy.on_shutdown(self.save_all_data)
        rospy.loginfo('Robot {}: Successfully created graph node'.format(self.robot_id))

    def compute_gvg(self):
        """Compute GVG for exploration."""

        start_time_clock = rospy.Time.now().to_sec()
        # Get only wall cells for the Voronoi.
        obstacles = self.latest_map.wall_cells()
        end_time_clock = rospy.Time.now().to_sec()
        pu.log_msg(self.robot_id,"generate obstacles2 {}".format(end_time_clock - start_time_clock),self.debug_mode)

        start_time_clock = rospy.Time.now().to_sec()
        # Get Voronoi diagram.
        vor = Voronoi(obstacles)
        end_time_clock = rospy.Time.now().to_sec()
        pu.log_msg(self.robot_id,"voronoi {}".format(end_time_clock - start_time_clock),self.debug_mode)
        start_time_clock = rospy.Time.now().to_sec()
        # Initializing the graph.
        self.graph = igraph.Graph()
        # Correspondance between graph vertex and Voronoi vertex IDs.
        voronoi_graph_correspondance = {}

        # Simplifying access of Voronoi data structures.
        vertices = vor.vertices
        ridge_vertices = vor.ridge_vertices
        ridge_points = vor.ridge_points

        edges = []
        weights = []
        # Create a graph based on ridges.
        for i in range(len(ridge_vertices)):
            ridge_vertex = ridge_vertices[i]
            # If any of the ridge vertices go to infinity, then don't add.
            if ridge_vertex[0] == -1 or ridge_vertex[1] == -1:
                continue
            p1 = vertices[ridge_vertex[0]]
            p2 = vertices[ridge_vertex[1]]

            # Obstacle points determining the ridge.
            ridge_point = ridge_points[i]
            q1 = obstacles[ridge_point[0]]
            q2 = obstacles[ridge_point[1]]

            # If the vertices on the ridge are in the free space
            # and distance between obstacle points is large enough for the robot
            if self.latest_map.is_free(p1[INDEX_FOR_X], p1[INDEX_FOR_Y]) and \
                self.latest_map.is_free(p2[INDEX_FOR_X], p2[INDEX_FOR_Y]) and \
                    euclidean_distance(q1, q2) > self.min_edge_length:

                # Add vertex and edge.
                graph_vertex_ids = [-1, -1] # temporary for finding verted IDs.

                # Determining graph vertex ID if existing or not.
                for point_id in range(len(graph_vertex_ids)):
                    if ridge_vertex[point_id] not in voronoi_graph_correspondance:
                        # if not existing, add new vertex.
                        graph_vertex_ids[point_id] = self.graph.vcount()
                        self.graph.add_vertex(
                            coord=vertices[ridge_vertex[point_id]])
                        voronoi_graph_correspondance[ridge_vertex[point_id]] = graph_vertex_ids[point_id]
                    else:
                        # Otherwise, already added before.
                        graph_vertex_ids[point_id] = voronoi_graph_correspondance[ridge_vertex[point_id]]

                # Add edge.
                self.graph.add_edge(graph_vertex_ids[0], graph_vertex_ids[1],
                    weight=euclidean_distance(p1, p2))
            else:
                # Otherwise, edge not added.
                continue

        # Take only the largest component.
        cl = self.graph.clusters()
        self.graph = cl.giant()

        # self.get_adjacency_list(self.edges)
        # self.connect_subtrees()

        self.prune_leaves()
        end_time_clock = rospy.Time.now().to_sec()
        pu.log_msg(self.robot_id,"ridge {}".format(end_time_clock - start_time_clock),self.debug_mode)

        # Publish GVG.
        self.publish_edges()

    def count_path_vertices(self, vid1,vid2):
        result=self.graph.shortest_paths(source=[vid1], target=[vid2])
        # pu.log_msg(self.robot_id,"Edge connectivity between {} and {}: {}".format(vid1,vid2,result),self.debug_mode)
        return result[0][0]

    def merge_similar_edges2(self):
        current_vertex_id = 0 # traversing graph from vertex 0.

        self.leaves = {}
        while current_vertex_id < self.graph.vcount():
            if self.graph.degree(current_vertex_id) == 2:
                p_id, r_id = self.graph.neighbors(current_vertex_id)
                p = self.graph.vs["coord"][p_id]
                current_vertex = self.graph.vs["coord"][current_vertex_id]
                r = self.graph.vs["coord"][r_id]

                slope_pc = pu.get_slope(p, current_vertex)
                slope_cr = pu.get_slope(p, current_vertex)
                if np.isclose(slope_pc,
                    slope_cr, rtol=0.001, atol=0.001, equal_nan=True) or \
                    np.isclose(slope_cr, slope_pc, rtol=0.001, atol=0.001,
                        equal_nan=True): # TODO tune.
                    pq_id = self.graph.get_eid(p_id, current_vertex_id)
                    qr_id = self.graph.get_eid(current_vertex_id, r_id)

                    self.graph.add_edge(p_id, r_id,
                        weight=self.graph.es["weight"][pq_id]+self.graph.es["weight"][qr_id])
                    self.graph.delete_vertices(current_vertex_id)
                else:

                    current_vertex_id += 1
            else:
                if self.graph.degree(current_vertex_id) == 1:
                    self.leaves[current_vertex_id] = self.latest_map.unknown_area_approximate(current_vertex)
                current_vertex_id += 1

    def prune_leaves(self):
        current_vertex_id = 0 # traversing graph from vertex 0.
        self.leaves = {}
        self.intersections = {}
        while current_vertex_id < self.graph.vcount():
            if self.graph.degree(current_vertex_id) == 1:
                neighbor_id = self.graph.neighbors(current_vertex_id)[0]
                neighbor = self.graph.vs["coord"][neighbor_id]
                current_vertex = self.graph.vs["coord"][current_vertex_id]
                if not self.latest_map.is_frontier(
                    neighbor, current_vertex, self.min_range_radius): # TODO parameter.
                    self.graph.delete_vertices(current_vertex_id)
                else:
                    self.leaves[current_vertex_id] = self.latest_map.unknown_area_approximate(current_vertex)
                    current_vertex_id += 1
            else:
                if self.graph.degree(current_vertex_id) > 2:
                    self.intersections[current_vertex_id] = self.graph.vs["coord"][current_vertex_id]
                current_vertex_id += 1
        self.publish_leaves()

    def publish_edges(self):
        """For debug, publishing of GVG."""

        # Marker that will contain line sequences.
        m = Marker()
        m.id = 0
        m.header.frame_id = self.latest_map.header.frame_id
        m.type = Marker.LINE_LIST
        # TODO constant values set at the top.
        m.color.a = 1.0
        m.color.r = 1.0
        m.scale.x = 0.1

        # Plot each edge.
        for edge in self.graph.get_edgelist():
            for vertex_id in edge:
                # Grid coordinate for the vertex.
                p = self.graph.vs["coord"][vertex_id]
                p_t = self.latest_map.grid_to_pose(p)
                p_ros = Point(x=p_t[0], y=p_t[1])

                m.points.append(p_ros)

        # Publish marker.
        self.marker_pub.publish(m)

    def publish_current_vertex(self, vertex_id, marker_id=1):
        # Publish current vertex
        m = Marker()
        m.id = marker_id
        m.header.frame_id = self.latest_map.header.frame_id
        m.type = Marker.SPHERE
        m.color.a = 1.0
        m.color.g = 1.0
        if marker_id == 5:
            m.color.b = 1.0
        # TODO constant values set at the top.
        m.scale.x = 0.5
        m.scale.y = 0.5
        #m.scale.x = 0.1
        p = self.graph.vs["coord"][vertex_id]
        p_t = self.latest_map.grid_to_pose(p)
        p_ros = Point(x=p_t[0], y=p_t[1])
        m.pose.position = p_ros
        self.marker_pub.publish(m)

    def publish_visited_vertices(self):

        # Publish visited vertices
        m = Marker()
        m.id = 2
        m.header.frame_id = self.latest_map.header.frame_id
        m.type = Marker.POINTS
        # TODO constant values set at the top.
        m.color.a = 1.0
        m.color.b = 1.0
        m.scale.x = 0.5
        m.scale.y = 0.5
        for vertex_id in self.visited_vertices:
            # Grid coordinate for the vertex.
            p = self.graph.vs["coord"][vertex_id]
            p_t = self.latest_map.grid_to_pose(p)
            p_ros = Point(x=p_t[0], y=p_t[1])
            m.points.append(p_ros)
        self.marker_pub.publish(m)

    def publish_leaves(self):
        # Publish leaves
        if self.leaves:
            m = Marker()
            m.id = 3
            m.header.frame_id = self.latest_map.header.frame_id
            m.type = Marker.POINTS
            # TODO constant values set at the top.
            m.color.a = 1.0
            m.color.r = 1.0
            m.color.b = 1.0
            m.scale.x = 0.5
            m.scale.y = 0.5
            for vertex_id in self.leaves:
                # Grid coordinate for the vertex.
                p = self.graph.vs["coord"][vertex_id]
                p_t = self.latest_map.grid_to_pose(p)
                p_ros = Point(x=p_t[0], y=p_t[1])
                m.points.append(p_ros)
            self.marker_pub.publish(m)

    def publish_line(self, p, q):
        m = Marker()
        m.id = 7
        m.header.frame_id = self.latest_map.header.frame_id
        m.type = Marker.LINE_STRIP
        # TODO constant values set at the top.
        m.color.a = 1.0
        m.color.r = 1.0
        m.scale.x = 0.1
        p_t = self.latest_map.grid_to_pose(p)
        p_ros = Point(x=p_t[0], y=p_t[1])
        m.points.append(p_ros)
        q_t = self.latest_map.grid_to_pose(q)
        q_ros = Point(x=q_t[0], y=q_t[1])
        m.points.append(q_ros)
        self.marker_pub.publish(m)

    def generate_graph(self):
        self.lock.acquire()
        start_time = time.time()
        try:
            self.latest_map = Grid(self.get_map().map)
        except rospy.ServiceException:
            self.lock.release()
            pu.log_msg(self.robot_id,"Map didn't update",self.debug_mode)
            return
        self.min_range_radius = 8 / self.latest_map.resolution # TODO range.
        self.min_edge_length = self.robot_radius / self.latest_map.resolution
        self.compute_gvg()
        self.lock.release()
        now = time.time()
        t = now - start_time
        self.performance_data.append(
            {'time': rospy.Time.now().to_sec(), 'type': 0, 'robot_id': self.robot_id, 'computational_time': t})

    def get_closest_vertex(self, robot_pose):
        """Get the closest vertex to robot_pose (x,y) in frame_id."""
        robot_grid = self.latest_map.pose_to_grid(robot_pose)
        vertices = np.asarray(self.graph.vs["coord"])
        rospy.logerr(robot_grid)
        return pu.get_closest_point(robot_grid, vertices)

    def get_successors(self, robot_pose, previous_pose=None):
        self.lock.acquire()
        start_time = rospy.Time.now().to_sec()
        # Get current vertex.
        self.current_vertex_id, distance_current_vertex = self.get_closest_vertex(robot_pose)
        current_vertex_id = self.current_vertex_id
        self.publish_current_vertex(self.current_vertex_id)

        # find visited vertices
        self.visited_vertices = set()
        previous_previous_vertex_id = -1
        previous_vertex_id = -1
        for p in previous_pose:
            p_vertex_id, distance_p = self.get_closest_vertex(p)
            if distance_p < 6 and p_vertex_id != current_vertex_id: # TODO parameter.
                self.visited_vertices.add(p_vertex_id)
                previous_previous_vertex_id = previous_vertex_id
                previous_vertex_id = p_vertex_id
                if len(self.visited_vertices) > 1:
                    path_prev_current = self.graph.get_shortest_paths(previous_previous_vertex_id, to=previous_vertex_id, weights=self.graph.es["weight"], mode=igraph.ALL)
                    self.visited_vertices.update(path_prev_current[0])

        if previous_vertex_id != -1:
            path_prev_current = self.graph.get_shortest_paths(previous_vertex_id,
                to=current_vertex_id, weights=self.graph.es["weight"],
                mode=igraph.ALL)
            self.visited_vertices.update(path_prev_current[0][:-1])
        self.publish_visited_vertices()

        # find next node
        non_visited_leaves = []
        for l_vertex_id in self.leaves:
            if l_vertex_id not in self.visited_vertices and l_vertex_id != self.current_vertex_id:
                non_visited_leaves.append(l_vertex_id)

        # If current vertex is a leaf, then continue in that direction.
        if current_vertex_id in non_visited_leaves:
            self.lock.release()
            return [self.latest_map.grid_to_pose(
                            self.graph.vs["coord"][current_vertex_id])]

        # Find other leaves to visit
        paths_to_leaves = self.graph.get_shortest_paths(current_vertex_id,
            to=non_visited_leaves, weights=self.graph.es["weight"],
            mode=igraph.ALL)

        full_path = []
        if paths_to_leaves:
            path_costs = [0] * len(paths_to_leaves)
            depth = 1
            path_to_leaf = -1
            path_length = np.inf
            max_depth = max([len(path) for path in paths_to_leaves])
            while depth < max_depth:
                counter = 0
                for i, path in enumerate(paths_to_leaves):
                    if depth < len(path):
                        if path_to_leaf != i:
                            path_costs[i] += self.graph.es["weight"][self.graph.get_eid(path[depth-1], path[depth])]

                            if path[depth] not in self.visited_vertices:
                                if (path_to_leaf == -1 or path_costs[i] < path_costs[path_to_leaf]):
                                    path_to_leaf = i
                    if path_to_leaf != -1 and path_costs[i] > path_costs[path_to_leaf]:
                        counter += 1

                if counter == len(paths_to_leaves):
                    break
                depth += 1
            if path_to_leaf == -1:
                path_to_leaf = np.argmin(path_costs)

            for v in paths_to_leaves[path_to_leaf]:
                full_path.append(self.latest_map.grid_to_pose(
                    self.graph.vs["coord"][v]))
        self.lock.release()
        return full_path

    def find_similar_slope_vertices(self, vertex_id):
        similar_slope_vertices = {}
        self.find_similar_slope_vertices_helper(vertex_id, similar_slope_vertices)

        return np.array(similar_slope_vertices.values())

    def find_similar_slope_vertices_helper(self, current_vertex_id, similar_slope_vertices):
        similar_slope_vertices[current_vertex_id] = self.graph.vs["coord"][current_vertex_id]
        if self.graph.degree(current_vertex_id) == 2:
            p_id, r_id = self.graph.neighbors(current_vertex_id)

            p = self.graph.vs["coord"][p_id]
            current_vertex = self.graph.vs["coord"][current_vertex_id]
            r = self.graph.vs["coord"][r_id]

            slope_pc = pu.get_slope(p, current_vertex)
            slope_cr = pu.get_slope(p, current_vertex)
            if np.isclose(slope_pc,
                slope_cr, rtol=0.001, atol=0.001, equal_nan=True) or \
                np.isclose(slope_cr, slope_pc, rtol=0.001, atol=0.001,
                    equal_nan=True): # TODO tune.

                if p_id not in similar_slope_vertices:
                    self.find_similar_slope_vertices_helper(p_id, similar_slope_vertices)
                if r_id not in similar_slope_vertices:
                    self.find_similar_slope_vertices_helper(r_id, similar_slope_vertices)
        elif self.graph.degree(current_vertex_id) == 1:
            p_id = self.graph.neighbors(current_vertex_id)
            if p_id[0] not in similar_slope_vertices:
                self.find_similar_slope_vertices_helper(p_id[0], similar_slope_vertices)

    def should_communicate(self, robot_pose):
        """Return True if should communicate; false otherwise."""
        self.lock.acquire()
        start_time = rospy.Time.now().to_sec()

        #self.merge_similar_edges2()

        # Get current vertex of the robot.
        self.current_vertex_id, distance_current_vertex = self.get_closest_vertex(robot_pose)
        current_vertex_id = self.current_vertex_id
        robot_grid = self.graph.vs["coord"][current_vertex_id]

        # self.publish_current_vertex(current_vertex_id, marker_id=5)
        # If at a junction point, might be worth to communicate.
        if self.graph.degree(self.current_vertex_id) > 2:
            pu.log_msg(self.robot_id,"intersection",self.debug_mode)
            self.lock.release()
            return True
        else:
            vertices = np.array(self.intersections.values())

            if vertices.size != 0:
                closest_intersection_point, dist = pu.get_closest_point(robot_grid, vertices)
                if dist < 5:# TODO parameter
                    pu.log_msg(self.robot_id,"intersection",self.debug_mode)
                    self.lock.release()
                    return True

        # Points on the same line for current_vertex_id
        current_vertex_similar_slope_vertices = self.find_similar_slope_vertices(current_vertex_id)
        #rospy.logerr("current_vertex_similar_slope_vertices {}".format(len(current_vertex_similar_slope_vertices)))
        # If can connect with another end, might be worth to communicate.
        for l_vertex_id in self.leaves:
            if l_vertex_id != current_vertex_id:
                rospy.sleep(0.01)
                # self.publish_current_vertex(l_vertex_id, marker_id=6) # TODO disable

                if pu.euclidean_distance(self.graph.vs["coord"][current_vertex_id], self.graph.vs["coord"][l_vertex_id]) < 30 / self.latest_map.resolution: # TODO parameter for communication range
                    l_vertex_similar_slope_vertices = self.find_similar_slope_vertices(l_vertex_id)

                    try:
                        line_regress = pu.get_line(np.vstack((current_vertex_similar_slope_vertices, l_vertex_similar_slope_vertices)))
                    except ValueError:
                        continue

                    if line_regress[2] < 2.0 and self.latest_map.line_in_unknown(self.graph.vs["coord"][current_vertex_id], self.graph.vs["coord"][l_vertex_id]): # TODO parameter:
                        #rospy.logerr("line rmse {} {}".format(line_regress[2], np.vstack((current_vertex_similar_slope_vertices, l_vertex_similar_slope_vertices))))
                        p = [self.graph.vs["coord"][current_vertex_id][0], line_regress[1] + line_regress[0] * self.graph.vs["coord"][current_vertex_id][0]]
                        q = [self.graph.vs["coord"][l_vertex_id][0], line_regress[1] + line_regress[0] * self.graph.vs["coord"][l_vertex_id][0]]
                        self.publish_line(p, q) # TODO disable
                        self.lock.release()
                        return True
        end_time = rospy.Time.now().to_sec()
        t = (end_time - start_time)
        pu.log_msg(self.robot_id,"should_communicate {}".format(t),self.debug_mode)
        self.performance_data.append(
            {'time': rospy.Time.now().to_sec(), 'type': 1, 'robot_id': self.robot_id, 'computational_time': t}) # TODO not done if true
        self.lock.release()
        return False

    #####

    def frontier_point_handler(self, request):
        pu.log_msg(self.robot_id,"received a request: {}".format(request.count),self.debug_mode)
        count = request.count
        self.generate_graph()
        start_time = rospy.Time.now().to_sec()
        if (self.leaves):
            best_leaf= max(self.leaves,key=self.leaves.get)
            rospy.logerr(best_leaf)
            selected_leaves = [best_leaf]
            scount=0
            while scount <= count:
                if len(selected_leaves)==len(self.leaves):
                    break
                for lid,area in self.leaves.items():
                    if lid not in selected_leaves:
                        v_dict ={}
                        for slid in selected_leaves:
                            v_count=self.count_path_vertices(lid,slid)
                            v_dict[v_count]=lid
                if v_dict:
                    max_val=max(list(v_dict))
                    max_lid = v_dict[max_val]
                    selected_leaves.append(max_lid)
                scount+=1

        frontiers = []
        for leaf_id in selected_leaves:
            p = self.latest_map.grid_to_pose(self.graph.vs["coord"][leaf_id])
            p_ros = Pose()
            p_ros.position.x = p[0]
            p_ros.position.y = p[1]
            frontiers.append(p_ros)
        now = rospy.Time.now().to_sec()
        t = (now - start_time)
        pu.log_msg(self.robot_id,'COMPUTED FRONTIER RESULTS: {}'.format(frontiers),self.debug_mode)
        return FrontierPointResponse(frontiers=frontiers)

    def intersection_handler(self, data):
        pose_data = data.pose
        pu.log_msg(self.robot_id, "Received Intersection request", self.debug_mode)
        if not self.edges or self.enough_delay():
            self.generate_graph()
        robot_pose = [0.0] * 2
        robot_pose[INDEX_FOR_X] = pose_data.position.x
        robot_pose[INDEX_FOR_Y] = pose_data.position.y
        result = self.should_communicate(np.array(robot_pose))
        return IntersectionsResponse(result=result)

    def fetch_explored_region_handler(self, data):
        '''
        Map Analyzer use this to fetch explored cells
        :param data:
        :return:
        '''
        poses=self.latest_map.get_explored_region()
        return ExploredRegionResponse(poses=poses, resolution=self.map_resolution)

    ###### TO CHECK what is needed.

    def map_callback(self, map_msg):
        """Callback for Occupancy Grid."""
        pu.log_msg(self.robot_id,"Received map message",self.debug_mode)
        start_time_clock = rospy.Time.now().to_sec()
        # Create a 2D grid.
        self.latest_map = Grid(map_msg)
        # Adjust min distance between obstacles in cells.
        self.min_edge_length = self.robot_radius / self.latest_map.resolution
        end_time_clock = rospy.Time.now().to_sec()
        pu.log_msg(self.robot_id,"generate obstacles1 {}".format(end_time_clock - start_time_clock),self.debug_mode)
        # just for testing
        self.generate_graph()
        pu.log_msg(self.robot_id,"should comm {}".format(self.should_communicate(self.get_robot_pose())),self.debug_mode)
        """
        if not self.plot_data_active:
            self.plot_data([], is_initial=True)
            rospy.logerr('Plotting complete')
        """

    def spin(self):
        r = rospy.Rate(0.1)
        while not rospy.is_shutdown():
            try:
                #if not self.edges:
                #    self.generate_graph()
                r.sleep()
            except Exception as e:
                pu.log_msg(self.robot_id,'Robot {}: Graph node interrupted!: {}'.format(self.robot_id, e),self.debug_mode)

    def is_same_intersection(self, intersec, robot_pose):
        is_same = True
        if self.last_intersection:
            last_close_edge = self.last_intersection[0]
            current_close_edge = intersec[0]
            last_dist = min([pu.D(robot_pose, last_close_edge[0]), pu.D(robot_pose, last_close_edge[1])])
            current_dist = min([pu.D(robot_pose, current_close_edge[0]), pu.D(robot_pose, current_close_edge[1])])
            if abs(current_dist - last_dist) > self.comm_range:
                is_same = False
        return is_same

    def fetch_edge_handler(self, data):
        self.generate_graph()
        pose = [0.0] * 2
        pose[INDEX_FOR_X] = data.pose.position.x
        pose[INDEX_FOR_Y] = data.pose.position.y
        robot_pose = pu.scale_up(pose, self.graph_scale)
        edgelist = self.create_edge_list(robot_pose)
        return FetchGraphResponse(edgelist=edgelist)

    def enough_delay(self):
        return rospy.Time.now().to_sec() - self.last_graph_update_time > 30  # updated last 20 secs

    def create_edge_list(self, robot_pose):
        alledges = list(self.edges)
        edgelist = EdgeList()
        edgelist.header.stamp = rospy.Time.now()
        edgelist.header.frame_id = str(self.robot_id)
        edgelist.ridges = []
        edgelist.pixels = []
        edge_dists = {}
        for e in alledges:
            if e in self.edges:
                obs = self.edges[e]
                ridge = self.create_ridge((e, obs))
                edgelist.ridges.append(ridge)
                if self.in_line_with_previous_edge(self.prev_ridge, (e, obs)):
                    d = pu.D(robot_pose, e[1])  # min([pu.D(robot_pose, e[0]), pu.D(robot_pose, e[1])])
                    edge_dists[(e, obs)] = d
        if edge_dists:
            new_close_ridge = min(edge_dists, key=edge_dists.get)
        else:
            new_close_ridge = ((self.prev_ridge[0][1], self.prev_ridge[0][0]), self.prev_ridge[1])
        new_closest_ridge = self.create_ridge(new_close_ridge)
        edgelist.close_ridge = new_closest_ridge
        self.prev_ridge = new_close_ridge
        for k, v in self.pixel_desc.items():
            if v != FREE:
                pix = Pixel()
                pix.pose.position.x = k[INDEX_FOR_X]
                pix.pose.position.y = k[INDEX_FOR_Y]
                pix.desc = v
                edgelist.pixels.append(pix)
        return edgelist

    def fetch_rendezvous_points_handler(self, data):
        count = data.count
        rendezvous_points = []
        all_points = self.latest_map.get_explored_region()
        origin = self.get_robot_pose()
        free_points = [p for p in all_points if pu.D(origin, p) <= self.comm_range]
        if free_points:
            hull, boundary_points = graham_scan(free_points, count, False)
            for ap in boundary_points:
                pose = Pose()
                pose.position.x = ap[INDEX_FOR_X]
                pose.position.y = ap[INDEX_FOR_Y]
                rendezvous_points.append(pose)
        return RendezvousPointsResponse(poses=rendezvous_points)

    def get_closest_edge(self, robot_pose, close_edges):
        linear_edge = {}
        chosen_edge = None
        for e in close_edges:
            if self.in_line_with_previous_edge(self.prev_ridge, e):
                d = pu.D(robot_pose, e[0][1])
                linear_edge[d] = e
        if linear_edge:
            chosen_edge = linear_edge[min(linear_edge.keys())]
        return chosen_edge

    # def round_point(self, p):
    #     xc = round(p[INDEX_FOR_X], 2)
    #     yc = round(p[INDEX_FOR_Y], 2)
    #     new_p = [0.0] * 2
    #     new_p[INDEX_FOR_X] = xc
    #     new_p[INDEX_FOR_Y] = yc
    #     new_p = tuple(new_p)
    #     return new_p

    def distance_to_line(self, p1, p2, pose):
        p2_p1_vec = pu.get_vector(p1, p2)
        pose_p1_vec = pu.get_vector(p1, pose)
        if p2_p1_vec[0] != 0 and p2_p1_vec[1] != 0:
            d = norm(np.cross(p2_p1_vec, pose_p1_vec)) / norm(p2_p1_vec)
        else:
            d = INF
        return d

    def get_pairs_with_unknown_area(self, edge_pairs):
        edge_boxes = {}
        pair_dist = {}
        intersections = []
        for dist, pair in edge_pairs.items():
            p1 = pair[0][1]
            p2 = pair[1][1]
            x_min = min([p1[INDEX_FOR_X], p2[INDEX_FOR_X]])
            y_min = min([p1[INDEX_FOR_Y], p2[INDEX_FOR_Y]])
            x_max = max([p1[INDEX_FOR_X], p2[INDEX_FOR_X]])
            y_max = max([p1[INDEX_FOR_Y], p2[INDEX_FOR_Y]])
            min_points = max([abs(x_max - x_min), abs(y_max - y_min)])
            bbox = sg.box(x_min, y_min, x_max, y_max)
            edge_boxes[pair] = (bbox, min_points)
            pair_dist[pair] = dist
        if edge_boxes:
            actual_pairs = {}
            actual_intersecs = {}
            for p, v in self.pixel_desc.items():
                if v == UNKNOWN:
                    point = sg.Point(p[INDEX_FOR_X], p[INDEX_FOR_Y])
                    for pair, values in edge_boxes.items():
                        if values[0].contains(point):
                            if pair not in actual_intersecs:
                                actual_intersecs[pair] = 1
                            else:
                                actual_intersecs[pair] += 1
            for pair, count in actual_intersecs.items():
                if count >= edge_boxes[pair][1]:
                    actual_pairs[pair_dist[pair]] = pair
            if actual_pairs:
                min_dist = min(actual_pairs.keys())
                intersections.append(actual_pairs[min_dist])
        return intersections

    def get_intersection(self, robot_pose, e1, e2, o1, o2):
        p1 = e1[0]
        p2 = e1[1]
        v1 = pu.get_vector(p1, p2)
        width1 = pu.D(o1[0], o1[1])
        p3 = e2[0]
        p4 = e2[1]
        width2 = pu.D(o2[0], o2[1])
        v2 = pu.get_vector(p3, p4)
        intersec = None
        if self.lidar_scan_radius < pu.D(p2, p4) < self.comm_range:
            if pu.there_is_unknown_region(p2, p4, self.pixel_desc):
                if pu.collinear(p1, p2, p4, width1, self.slope_bias):
                    cos_theta, separation = pu.compute_similarity(v1, v2, e1, e2)
                    if -1 <= cos_theta <= -1 + self.opposite_vector_bias and abs(
                            separation - width1) < self.separation_bias:
                        intersec = (e1, e2)
                    else:
                        pass
                else:
                    pass
            else:
                pass
        return intersec

    def has_unknown_points(self, p1,p2):
        line_points= self.get_line(p1[INDEX_FOR_X],p1[INDEX_FOR_Y],p2[INDEX_FOR_X],p2[INDEX_FOR_Y])
        for p in line_points:
            if not self.is_free(p):
                return True
        return False

    def get_line(self,x1, y1, x2, y2):
        x1=int(round(x1))
        y1=int(round(y1))
        x2=int(round(x2))
        y2=int(round(y2))
        points = []
        issteep = abs(y2-y1) > abs(x2-x1)
        if issteep:
            x1, y1 = y1, x1
            x2, y2 = y2, x2
        rev = False
        if x1 > x2:
            x1, x2 = x2, x1
            y1, y2 = y2, y1
            rev = True
        deltax = x2 - x1
        deltay = abs(y2-y1)
        error = int(deltax / 2)
        y = y1
        ystep = None
        if y1 < y2:
            ystep = 1
        else:
            ystep = -1
        for x in range(x1, x2 + 1):
            if issteep:
                points.append((y, x))
            else:
                points.append((x, y))
            error -= deltay
            if error < 0:
                y += ystep
                error += deltax
        if rev:
            points.reverse()
        return points

    def merge_graphs(self):
        if self.old_edges and self.edges:
            edge1, subtree1, parents1 = self.localize_robot(self.robot_pose, self.old_adj_list, self.old_edges)
            edge2, subtree2, parents2 = self.localize_robot(self.robot_pose, self.adj_list, self.edges)
            edge_pair = (edge1, edge2)
            self.remove_subtrees(edge1, self.old_edges, self.old_adj_list)
            self.remove_subtrees(edge2, self.edges, self.adj_list)
            self.link_edges(edge_pair, self.old_edges, self.edges)
            self.get_adjacency_list(self.edges)
        else:
            pu.log_msg(self.robot_id,"empty subtree",self.debug_mode)

    def already_exists(self, p):
        pose = [0.0] * 2
        pose[INDEX_FOR_X] = round(p[INDEX_FOR_X])
        pose[INDEX_FOR_Y] = round(p[INDEX_FOR_Y])
        pose = tuple(pose)
        return pose in self.deleted_nodes

    def localize_robot(self, robot_pose, adj_list, edges):
        dists = {}
        s = list(adj_list.keys())[0]
        visited = {}
        parents = {s: None}
        S = [s]
        while len(S) > 0:
            u = S.pop()
            neighbors = adj_list[u]
            for v in neighbors:
                if v not in visited:
                    S.append(v)
                    parents[v] = u
                    dists[pu.D(robot_pose, v)] = (u, v)
            visited[u] = None
        close_edge = dists[min(dists.keys())]
        subtree, parents = self.get_subtree(close_edge, adj_list)
        return close_edge, subtree, parents

    def remove_subtrees(self, close_edge, edges, adj_list):
        visited = [close_edge[0]]
        S = [close_edge[1]]
        while len(S) > 0:
            u = S.pop()
            neighbors = adj_list[u]
            for v in neighbors:
                if v not in visited:
                    S.append(v)
                    e = (u, v)
                    if e != close_edge:
                        if e in edges:
                            del edges[e]
            visited.append(u)

    def link_edges(self, edge_pair, edges1, edges2):
        e1 = edge_pair[0]
        e2 = edge_pair[1]
        if e1 in edges1:
            obs1 = edges1[e1]
        else:
            obs1 = edges1[(e1[1], e1[0])]
        if e2 in edges2:
            obs2 = edges2[e2]
        else:
            obs2 = edges2[(e2[1], e2[0])]
        obs = obs1
        w1 = pu.D(obs1[0], obs1[1])
        w2 = pu.D(obs2[0], obs2[1])
        if w2 > w1:
            obs = obs2

        new_edge = (e1[1], e2[1])
        del edges1[e1]
        del edges2[e2]
        edges2[new_edge] = obs
        edges2.update(edges1)

    def get_subtree(self, edge, adj_list):
        S = [edge[1]]
        visited = [edge[0]]
        parents = {edge[1]: edge[0]}
        leaves = set()
        while len(S) > 0:
            u = S.pop()
            neighbors = adj_list[u]
            if len(neighbors) <= 1:
                leaves.add(u)
            for v in neighbors:
                if v not in visited:
                    S.append(v)
                    parents[v] = u
            visited.append(u)
        tree = set()
        for l in leaves:
            self.get_path(l, parents[l], parents, tree)
        subtree = list(tree)
        return subtree, parents

    def get_path(self, leaf, parent, parents, tree):
        l = leaf
        while parents[l] != parent:
            l = parents[l]
            tree.add(l)
        tree.add(l)

    def merge_records(self):
        old_nodes = list(self.adj_list)
        new_leaves = list(self.leaf_slope)
        dist_leaf = {}
        obstacle_dict = {}
        for n in old_nodes:
            neighbors = self.adj_list[n]
            obs = None
            for a in neighbors:
                if (n, a) in self.edges:
                    obs = self.edges[(n, a)]
                    break
            if obs:
                obstacle_dict[n] = obs
                for nl in new_leaves:
                    if nl in dist_leaf:
                        dist_leaf[nl][pu.D(nl, n)] = n
                    else:
                        dist_leaf[nl] = {pu.D(nl, n): n}
        neighbor_distance = {}
        obs_dict = {}
        for k, v in dist_leaf.items():
            dist = np.min(v.keys())
            neighbor_distance[(v[dist], k)] = dist
            obs_dict[(v[dist], k)] = obstacle_dict[v[dist]]
        if neighbor_distance:
            min_dist_key = min(neighbor_distance, key=neighbor_distance.get)
            self.edges[min_dist_key] = obs_dict[min_dist_key]
            self.get_adjacency_list(self.edges)

    def connect_subtrees(self, count=0):
        N = len(self.adj_list)
        self.get_deeepest_tree_source()
        if not N:
            return
        if N == len(self.adj_dict[self.longest]):  # or count == 10:
            self.get_adjacency_list(self.edges)
            return
        allnodes = []  # all leaves for every search
        for k, v in self.leave_dict.items():
            allnodes += v
        allnodes = list(set(allnodes))  # deal with only unique leaves
        for leaf, adj in self.adj_dict.items():
            adj_leaves = self.leave_dict[leaf]
            leaf_dist = {}
            for l in adj_leaves:
                dist_dict = {}
                for n2 in allnodes:
                    if n2 not in adj and n2 not in adj_leaves:
                        dist_dict[n2] = pu.D(l, n2)
                if dist_dict:
                    nei = min(dist_dict, key=dist_dict.get)
                    leaf_dist[l] = nei
            longest_dist = {k: pu.D(k, v) for k, v in leaf_dist.items()}
            if longest_dist:
                closest = min(longest_dist, key=longest_dist.get)
                close_node = leaf_dist[closest]
                cl_n = self.adj_list[close_node]
                cls_n = self.adj_list[closest]
                self.adj_list[closest].add(close_node)
                self.adj_list[close_node].add(closest)
                w1 = 0
                w2 = 0
                clw1 = None
                clw2 = None
                for cl in cl_n:
                    clw1 = (close_node, cl)
                    if clw1 in self.edges:
                        cd = self.edges[(close_node, cl)]
                        w1 = pu.D(cd[0], cd[1])
                        break
                for ad in cls_n:
                    clw2 = (closest, ad)
                    if (closest, ad) in self.edges:
                        cd1 = self.edges[(closest, ad)]

                        w2 = pu.D(cd1[0], cd1[1])
                        break
                if w1 > w2:
                    self.edges[(close_node, closest)] = clw1
                else:
                    self.edges[(close_node, closest)] = clw2
            else:
                self.get_adjacency_list(self.edges)
                return
        self.get_adjacency_list(self.edges)
        return self.connect_subtrees(count=count + 1)

    def get_adjacency_list(self, edge_dict):
        edge_list = list(edge_dict)
        self.adj_list.clear()
        self.leaf_slope.clear()
        self.global_leaves.clear()
        self.leaf_edges.clear()
        for e in edge_list:
            first = e[0]
            second = e[1]
            if first != second:
                if first in self.adj_list:
                    self.adj_list[first].add(second)
                else:
                    self.adj_list[first] = {second}
                if second in self.adj_list:
                    self.adj_list[second].add(first)
                else:
                    self.adj_list[second] = {first}
            else:
                del edge_dict[e]
        for k, v in self.adj_list.items():
            if len(v) == 1:
                parent = list(v)[0]
                self.leaf_slope[k] = pu.theta(parent, k)
                self.global_leaves[k] = parent
                if (parent, k) in self.edges:
                    self.leaf_edges[k] = (parent, k)
                    self.leaf_obstacles[k] = self.edges[(parent, k)]

    def is_free(self, p):
        xc = int(np.round(p[INDEX_FOR_X]))
        yc = int(np.round(p[INDEX_FOR_Y]))
        new_p = [0] * 2
        new_p[INDEX_FOR_X] = xc
        new_p[INDEX_FOR_Y] = yc
        new_p = tuple(new_p)
        return new_p in self.pixel_desc and self.pixel_desc[new_p] == FREE

    def get_edge(self, ridge):
        edge = {}
        try:
            p1 = [0.0] * 2
            p1[INDEX_FOR_X] = ridge.nodes[0][INDEX_FOR_X]
            p1[INDEX_FOR_Y] = ridge.nodes[0][INDEX_FOR_Y]
            p1 = tuple(p1)
            p2 = [0.0] * 2
            p2[INDEX_FOR_X] = ridge.nodes[1][INDEX_FOR_X]
            p2[INDEX_FOR_Y] = ridge.nodes[1][INDEX_FOR_Y]
            p2 = tuple(p2)
            q1 = [0.0] * 2
            q1[INDEX_FOR_X] = ridge.obstacles[0][INDEX_FOR_X]
            q1[INDEX_FOR_Y] = ridge.obstacles[0][INDEX_FOR_Y]
            q1 = tuple(q1)
            q2 = [0.0] * 2
            q2[INDEX_FOR_X] = ridge.obstacles[1][INDEX_FOR_X]
            q2[INDEX_FOR_Y] = ridge.obstacles[1][INDEX_FOR_Y]
            q2 = tuple(q2)
            P = (p1, p2)
            Q = (q1, q2)
            edge[P] = Q
        except:
            pu.log_msg(self.robot_id, "Invalid goal", self.debug_mode)

        return edge

    def create_ridge(self, ridge):
        p1 = Pose()
        p1.position.x = ridge[0][0][INDEX_FOR_X]
        p1.position.y = ridge[0][0][INDEX_FOR_Y]

        p2 = Pose()
        p2.position.x = ridge[0][1][INDEX_FOR_X]
        p2.position.y = ridge[0][1][INDEX_FOR_Y]

        q1 = Pose()
        q1.position.x = ridge[1][0][INDEX_FOR_X]
        q1.position.y = ridge[1][0][INDEX_FOR_Y]

        q2 = Pose()
        q2.position.x = ridge[1][1][INDEX_FOR_X]
        q2.position.y = ridge[1][1][INDEX_FOR_Y]

        ridge = Ridge()
        ridge.nodes = [p1, p2]
        ridge.obstacles = [q1, q2]
        ridge.scale = self.graph_scale

        return ridge

    def in_line_with_previous_edge(self, prev_ridge, current_edge):
        in_line = False
        if not prev_ridge:
            in_line = True
        else:
            edge1 = prev_ridge[0]
            edge2 = current_edge[1]
            v1 = pu.get_vector(edge1[0], edge1[1])
            v2 = pu.get_vector(edge2[0], edge2[1])
            cos_theta, separation = pu.compute_similarity(v1, v2, edge1, edge2)
            if cos_theta >= 0:
                in_line = True
        return in_line

    def scatter_plot(self):
        plt.figure(figsize=(12, 9))
        ax = plt.subplot(111)
        ax.spines["top"].set_visible(False)
        ax.spines["right"].set_visible(False)
        ax.get_xaxis().tick_bottom()
        ax.get_yaxis().tick_left()
        plt.xticks(fontsize=FONT_SIZE)
        plt.yticks(fontsize=FONT_SIZE)
        ax.set_xlabel("X", fontsize=FONT_SIZE)
        ax.set_ylabel("Y", fontsize=FONT_SIZE)
        ax.tick_params(labelsize=FONT_SIZE)
        obstacles = list(self.obstacles)

        xr = [v[INDEX_FOR_Y] for v in obstacles]
        yr = [v[INDEX_FOR_X] for v in obstacles]
        ax.scatter(xr, yr, color='black', marker="1")
        x_pairs, y_pairs = pu.process_edges(self.edges)
        for i in range(len(x_pairs)):
            x = x_pairs[i]
            y = y_pairs[i]
            ax.plot(y, x, "g-o")
        plt.grid()
        plt.axis('off')
        plt.savefig("{}/map_update_{}_{}_{}.png".format(self.method, self.robot_id, time.time(), self.run))

        plt.close()
        # plt.show()

    def get_deeepest_tree_source(self):
        self.leave_dict.clear()
        self.adj_dict.clear()
        self.tree_size.clear()
        self.parent_dict.clear()
        leaves = list(self.leaf_slope)
        for s in leaves:
            S = [s]
            visited = {}
            parents = {s: None}
            lf = []
            node_adjlist = {}
            while len(S) > 0:
                u = S.pop()
                if u in self.adj_list:
                    neighbors = self.adj_list[u]
                    if u not in node_adjlist:
                        node_adjlist[u] = neighbors
                    if len(neighbors) == 1:
                        lf.append(u)
                    for v in neighbors:
                        if v not in visited:
                            S.append(v)
                            parents[v] = u
                    visited[u] = None
            self.adj_dict[s] = node_adjlist
            self.tree_size[s] = len(visited)
            self.leave_dict[s] = lf
            self.parent_dict[s] = parents
        if self.tree_size:
            self.longest = max(self.tree_size, key=self.tree_size.get)

    def merge_similar_edges(self):
        parents = {self.longest: None}
        deleted_nodes = {}
        S = [self.longest]
        visited = {}
        while len(S) > 0:
            u = S.pop()
            if u not in deleted_nodes:
                all_neis = []
                if u in self.adj_list:
                    all_neis = self.adj_list[u]
                neighbors = [k for k in all_neis if k != parents[u]]
                if len(neighbors) == 1:
                    v = neighbors[0]
                    if v not in visited:
                        S.append(v)
                        if parents[u]:
                            us = pu.get_vector(parents[u], u)
                            ps = pu.get_vector(u, v)
                            cos_theta, separation = pu.compute_similarity(us, ps, (parents[u], u), (u, v))
                            if 1 - self.opposite_vector_bias <= cos_theta <= 1:
                                parents[v] = parents[u]
                                deleted_nodes[u] = None
                                self.adj_list[v].remove(u)
                                self.adj_list[v].add(parents[u])
                                self.adj_list[parents[u]].add(v)
                                if (u, parents[u]) in self.edges:
                                    self.edges[(parents[u], v)] = self.edges[(u, parents[u])]
                                else:
                                    self.edges[(parents[u], v)] = self.edges[(parents[u], u)]
                                del self.adj_list[u]
                            else:
                                parents[v] = u
                        else:
                            parents[v] = u
                else:
                    for v in neighbors:
                        if v not in visited:
                            S.append(v)
                            parents[v] = u
                visited[u] = None

        new_adj_list = {}
        edges = {}
        for k, v in self.adj_list.items():
            if k not in new_adj_list:
                new_adj_list[k] = []
            for l in v:
                if l not in deleted_nodes:
                    new_adj_list[k].append(l)
                    if (k, l) in self.edges:
                        edges[(k, l)] = self.edges[(k, l)]
                    else:
                        edges[(k, l)] = self.edges[(l, k)]

        for k, v in new_adj_list.items():
            if len(v) == 1:
                self.leaf_slope[k] = pu.theta(k, list(v)[0])
        self.adj_list = new_adj_list
        self.edges = edges
        self.save_deleted_nodes(deleted_nodes)

    def save_deleted_nodes(self, deleted_nodes):
        for v, _ in deleted_nodes.items():
            pose = [0.0] * 2
            pose[INDEX_FOR_X] = round(v[INDEX_FOR_X])
            pose[INDEX_FOR_Y] = round(v[INDEX_FOR_Y])
            pose = tuple(pose)
            self.deleted_nodes[pose] = None  # add all the new deleted nodes

    def add_edge(self, edge, new_edges):
        obst = ((0, 0), (0, 0))
        p1 = edge[0]
        neighbors = self.adj_list[p1]
        for n in neighbors:
            if (p1, n) in self.edges:
                o = self.edges[(p1, n)]
                new_edges[edge] = o
                obst = 0
                break
            if (n, p1) in self.edges:
                o = self.edges[(n, p1)]
                new_edges[edge] = o
                obst = o
                break
        return obst

    def get_free_points(self):
        points = list(self.pixel_desc)
        return [p for p in points if self.pixel_desc[p] == FREE]

    def get_obstacles(self):
        points = list(self.pixel_desc)
        return [p for p in points if self.pixel_desc[p] == OCCUPIED]

    def compute_new_information(self):
        self.new_information.clear()
        pu.log_msg(self.robot_id,"leaves: {}".format(self.leaf_edges),self.debug_mode)
        leaf_obstacles = copy.deepcopy(self.leaf_obstacles)
        leaf_edges = copy.deepcopy(self.leaf_edges)
        polygons, start_end, points, unknown_points, marker_points = self.get_leaf_region(leaf_edges,
                                                                                leaf_obstacles)
        for leaf, edge in leaf_edges.items():
            if self.is_frontier({leaf: edge[0]}, points[leaf]):
                edge = leaf_edges[leaf]
                obs = leaf_obstacles[leaf]
                self.new_information[(edge, obs)] = unknown_points[leaf]

    def is_frontier(self, edge, leaf_region):
        leaf = list(edge.keys())[0]
        leaf = pu.get_point(leaf)
        full_cells = {pu.get_point(p): self.pixel_desc[p] for p in leaf_region}
        full_cells[leaf] = FREE
        frontiers = {}
        self.flood_fill(full_cells, None, leaf, [], frontiers)
        return len(frontiers) > 0

    def flood_fill(self, cells, prev_point, new_point, visited, frontiers):
        if new_point not in cells or new_point in visited:
            return
        if prev_point and cells[new_point] != cells[prev_point]:
            if cells[new_point] == UNKNOWN:
                frontiers[prev_point] = new_point
            return
        north_p = list(new_point)
        east_p = list(new_point)
        west_p = list(new_point)
        south_p = list(new_point)
        north_p[INDEX_FOR_X] += 1
        east_p[INDEX_FOR_X] -= 1
        south_p[INDEX_FOR_Y] += 1
        west_p[INDEX_FOR_Y] -= 1
        prev_point = new_point
        visited.append(new_point)

        self.flood_fill(cells, prev_point, tuple(north_p), visited, frontiers)
        self.flood_fill(cells, prev_point, tuple(east_p), visited, frontiers)
        self.flood_fill(cells, prev_point, tuple(south_p), visited, frontiers)
        self.flood_fill(cells, prev_point, tuple(west_p), visited, frontiers)

    def get_leaf_region(self, leaf_edges, leaf_obstacles):
        polygons = {}
        start_end = {}
        for leaf, edge in leaf_edges.items():
            parent = edge[0]
            obs = leaf_obstacles[leaf]
            width = pu.D(obs[0], obs[1])
            radius = self.lidar_scan_radius

            x = leaf[0]
            y = leaf[1]

            opp = width / 2.0
            adj = radius
            hyp = np.sqrt(opp ** 2 + adj ** 2)
            theta1 = pu.theta(parent, leaf)
            angle_sum = (np.pi / 2) + theta1
            cos_val = opp * np.cos(angle_sum)
            sin_val = opp * np.sin(angle_sum)

            top_left_x = x + cos_val
            top_left_y = y + sin_val

            bottom_left_x = x - cos_val
            bottom_left_y = y - sin_val

            lx = x + hyp * np.cos(theta1)
            ly = y + hyp * np.sin(theta1)

            top_right_x = lx + cos_val
            top_right_y = ly + sin_val

            bottom_right_x = lx - cos_val
            bottom_right_y = ly - sin_val

            polygon = Polygon([(bottom_left_x, bottom_left_y), (top_left_x, top_left_y), (top_right_x, top_right_y),
                               (bottom_right_x, bottom_right_y)])

            polygons[leaf] = polygon
            start_end[leaf] = (leaf, (lx, ly))

        points = {}
        unknown_points = {}
        leaf_keys = leaf_edges.keys()
        marker_points = []
        for pose, value in self.pixel_desc.items():
            point = sg.Point(pose[INDEX_FOR_X], pose[INDEX_FOR_Y])
            for leaf in leaf_keys:
                if polygons[leaf].contains(point):
                    marker_points.append(pose)
                    if leaf not in points:
                        points[leaf] = [pose]
                    else:
                        points[leaf].append(pose)
                    if value == UNKNOWN:
                        if leaf not in unknown_points:
                            unknown_points[leaf] = 1
                        else:
                            unknown_points[leaf] += 1
        return polygons, start_end, points, unknown_points, marker_points

    def area(self, point, orientation, radius):
        known_points = []
        unknown_points = []
        if radius > self.lidar_scan_radius:
            radius = self.lidar_scan_radius
        radius = int(round(radius))
        for d in np.arange(0, radius, 1):
            distance_points = []
            for theta in range(-1 * self.lidar_fov // 2, self.lidar_fov + 1):
                angle = np.deg2rad(theta) + orientation
                x = point[INDEX_FOR_X] + d * np.cos(angle)
                y = point[INDEX_FOR_Y] + d * np.sin(angle)
                pt = [0.0] * 2
                pt[INDEX_FOR_X] = x
                pt[INDEX_FOR_Y] = y
                pt = pu.get_point(pt)
                distance_points.append(pt)
            unique_points = set(distance_points)
            for p in unique_points:
                if not pu.is_unknown(p, self.pixel_desc):
                    known_points.append(p)
                else:
                    unknown_points.append(p)
        return known_points, unknown_points

    def plot_data(self, frontiers, is_initial=False, vertext=None):
        pu.log_msg(self.robot_id,"Creating plot started",self.debug_mode)
        self.plot_data_active = True
        plt.figure(figsize=(12, 9))
        ax = plt.subplot(111)
        ax.spines["top"].set_visible(False)
        ax.spines["right"].set_visible(False)
        ax.get_xaxis().tick_bottom()
        ax.get_yaxis().tick_left()
        plt.xticks(fontsize=FONT_SIZE)
        plt.yticks(fontsize=FONT_SIZE)
        ax.set_xlabel("X", fontsize=FONT_SIZE)
        ax.set_ylabel("Y", fontsize=FONT_SIZE)
        ax.tick_params(labelsize=FONT_SIZE)

        unknown_points = []
        pixels = list(self.pixel_desc)
        for p in pixels:
            v = self.pixel_desc[p]
            if v != FREE and v != OCCUPIED:
                unknown_points.append(p)
        if True:
            if unknown_points:
                UX, UY = zip(*unknown_points)
                ax.scatter(UX, UY, marker='1', color='gray')
            obstacles = list(self.obstacles)
            xr, yr = zip(*obstacles)
            ax.scatter(xr, yr, color='black', marker="1")
            x_pairs, y_pairs = pu.process_edges(self.edges)
            pu.log_msg(self.robot_id,"looping " + str(len(x_pairs)),self.debug_mode)
            for i in range(len(x_pairs)):
                x = x_pairs[i]
                y = y_pairs[i]
                ax.plot(x, y, "g-.")
            leaves = list(self.leaf_slope)
            """
            lx, ly = zip(*leaves)
            ax.scatter(lx, ly, color='red', marker='*', s=MARKER_SIZE)

            fx, fy = zip(*frontiers)
            ax.scatter(fx, fy, color='goldenrod', marker="D", s=MARKER_SIZE + 4)
            pose = self.get_robot_pose()
            point = pu.scale_up(pose, self.graph_scale)
            ax.scatter(point[INDEX_FOR_X], point[INDEX_FOR_Y], color='green', marker="D", s=MARKER_SIZE + 4)
            """
            plt.grid()
        #except:
        #    pass
        # plt.axis('off')
        pu.log_msg(self.robot_id,"looping " + str(self.method),self.debug_mode)
        plt.savefig("{}/plot_{}_{}_{}.png".format(self.method, self.robot_id, time.time(), self.run))
        plt.close()
        # plt.show()
        pu.log_msg(self.robot_id,"Creating plot completed",self.debug_mode)
        self.plot_data_active = False

    def plot_intersections(self, ax, ridge, intersections, point):
        self.plot_intersection_active = True
        obstacles = list(self.obstacles)
        fig, ax = plt.subplots(figsize=(16, 10))

        ox = [v[INDEX_FOR_X] for v in obstacles]
        oy = [v[INDEX_FOR_Y] for v in obstacles]
        current_x = []
        current_y = []
        if ridge:
            current_x = [ridge[0][INDEX_FOR_X], ridge[1][INDEX_FOR_X]]
            current_y = [ridge[0][INDEX_FOR_Y], ridge[1][INDEX_FOR_Y]]
        target_x = []
        target_y = []
        for section in intersections:
            Q = section[1]
            target_x += [Q[0][INDEX_FOR_X], Q[1][INDEX_FOR_X]]
            target_y += [Q[0][INDEX_FOR_Y], Q[1][INDEX_FOR_Y]]
        for i in range(len(target_x)):
            ax.plot(target_x[i], target_y[i], "r-d")

        for i in range(len(current_x)):
            ax.plot(current_x[i], current_y[i], "m-d")

        ax.scatter(point[INDEX_FOR_X], point[INDEX_FOR_Y], marker='*', color='purple')
        ax.scatter(ox, oy, color='black', marker='s')

        x_pairs, y_pairs = pu.process_edges(self.edges)
        for i in range(len(x_pairs)):
            x = x_pairs[i]
            y = y_pairs[i]
            ax.plot(x, y, "g-.")
        plt.grid()
        plt.savefig("{}/intersections_{}_{}_{}.png".format(self.method, self.robot_id, time.time(),
                                                           self.run))  # TODO consistent time.
        plt.close(fig)
        self.plot_intersection_active = False

    def save_all_data(self,data):
        save_data(self.performance_data,
                  "{}/performance_{}_{}_{}_{}_{}_{}.pickle".format(self.method, self.environment, self.robot_count,
                                                                self.run,
                                                                self.termination_metric, self.robot_id,self.max_target_info_ratio))

    def get_robot_pose(self):
        # TODO add in a different class.
        robot_pose = None
        while not robot_pose:
            if True:
                self.tf_listener.waitForTransform("robot_{}/map".format(self.robot_id),
                                             "robot_{}/base_link".format(self.robot_id),
                                             rospy.Time(0),
                                             rospy.Duration(4.0))
                (robot_loc_val, rot) = self.tf_listener.lookupTransform("robot_{}/map".format(self.robot_id),
                                                                     "robot_{}/base_link".format(self.robot_id),
                                                                     rospy.Time(0))
                robot_pose = robot_loc_val[0:2]
            #except:
            #    rospy.sleep(1)
            #    pass
        robot_pose = np.array(robot_pose)
        return robot_pose

if __name__ == "__main__":
    rospy.init_node("gvg_graph_node")

    graph = Graph()
    rospy.Subscriber('/robot_{}/map'.format(graph.robot_id),
        OccupancyGrid, graph.map_callback)
    graph.spin()


import requests
#import urllib.parse
import datetime

request_token_url = "https://api.twitter.com/oauth/request_token"
callback_url = ""

def createNewUser(api_key, api_secret):
	parameters = {
		"oauth_callback" : callback_url,
		"oauth_consumer_key" : api_key,
		"oauth_signature_method" : "HMAC-SHA1",
		"oauth_timestamp" : datetime.datetime.now().strftime('%Y-%m-%d %H:%M:%S'),
		"oauth_nonce" : datetime.datetime.now().strftime('%Y-%m-%d %H:%M:%S'),
		"oauth_version" : "1.0"
	}
def getUserInformation(auth):
	import requets

def registNewUser():
	print("input your API_KEY")
	api_key = raw_input("api_key >> ")

	print("input your API_SECRET")
	api_secret = raw_input("api_secret >> ")

	print("input your ACCESS_TOKEN")
	access_token = raw_input("access_token >> ")

	print("input your ACCESS_TOKEN_SECRET")
	access_token_secret = raw_input("access_token_secret >> ")


# This file implements the tippetop example,
# see Section 10.6 of "A nonsmooth generalized-alpha method for
# mechanical systems with frictional contact", Giuseppe Capobianco,
# Jonas Harsch, Simon R. Eugster, Remco I. Leine
# Int J Numer Methods Eng. 2021; 1– 30. https://doi.org/10.1002/nme.6801
#
#
# Stuttgart, November 2021                                  G.Capobianco

# ---- we need a better solution to this
import sys
from os import path
here = path.abspath(path.dirname(__file__))
root = path.abspath(path.dirname(path.dirname(here)))
sys.path.append(here)
sys.path.append(root)
# ==== we need a better solution to this

import numpy as np
from math import sin, cos, pi
import matplotlib.pyplot as plt

from tippetop_system import Tippetop_Leine2003, Tippetop_quaternion

from solvers.moreau import Moreau
from solvers.generalized_alpha import Generalized_alpha

if __name__ == "__main__":

    # show simulation results
    show_plots = True

    # Units: kg, m, s
    # Dynamics:
    m = 6e-3 # kg
    I1 = 8e-7 # kg m2 # = I_2 # Leine2013
    I3 = 7e-7 # kg m2
    g = 9.81 # kg m / s2
    # Geometry:
    a1 = 3e-3 # m
    a2 = 1.6e-2 # m
    r1 = 1.5e-2 # m
    r2 = 5e-3 # m

    mu = 0.3   # = mu1 = mu2
    eN = 0     # = eN1 = eN2
    eF = 0
    R = 5e-4   # m # = R1 = R2
    prox_r = 0.001

    # Initial conditions
    # all zeros exept:
    # Leine2003
    z0 = 1.2015e-2 # m
    theta0 = 0.1 # rad
    psi_dot0 = 180 # rad / s

    # top_g = Tippetop_Leine2003( m, I1, I3, g, a1, a2, r1, r2, R, eN, eF, mu, prox_r, z0, theta0, psi_dot0)
    top_g = Tippetop_quaternion(m, I1, I3, g, a1, a2, r1, r2, R, eN, eF, mu, prox_r, z0, theta0, psi_dot0)
    top_m = Tippetop_Leine2003( m, I1, I3, g, a1, a2, r1, r2, R, eN, eF, mu, prox_r, z0, theta0, psi_dot0)
    # top_m = Tippetop_quaternion( m, I1, I3, g, a1, a2, r1, r2, R, eN, eF, mu, prox_r, z0, theta0, psi_dot0)

    t0 = 0
    t1 = 8

    dt = 1e-3
    gen_al = Generalized_alpha(top_g, t0, t1, dt, rho_inf=0.5, newton_tol=1.0e-6)
    sol_g = gen_al.solve()

    t_g = sol_g[0]
    q_g = sol_g[1]
    u_g = sol_g[2]
    # a_g = sol_g[3]
    # La_N_g = sol_g[10]
    # la_N_g = sol_g[11]
    # La_F_g = sol_g[12]
    # la_F_g = sol_g[13]
    # P_N_g = sol_g[14]
    # P_F_g = sol_g[15]

    dt = 1e-4
    moreau = Moreau(top_m, t0, t1, dt, fix_point_tol=1e-6)
    sol_m = moreau.solve()

    t_m = sol_m[0]
    q_m = sol_m[1]
    u_m = sol_m[2]
    P_N_m = sol_m[5]
    P_F_m = sol_m[6]

    nt_g = len(t_g)

    # gaps and theta
    nt_m = len(t_m)
    g_N_g = np.zeros((nt_g, top_g.nla_N))
    theta_g = np.zeros(nt_g)
    g_N_m = np.zeros((nt_m, top_m.nla_N))
    theta_m = np.zeros(nt_m)

    for i, ti in enumerate(t_g):
        theta_g[i] = top_g.theta(ti, q_g[i])
        g_N_g[i] = top_g.g_N(ti, q_g[i])

    for i, ti in enumerate(t_m):
        theta_m[i] = top_m.theta(ti, q_m[i])
        g_N_m[i] = top_m.g_N(ti, q_m[i])

    if show_plots:

        # plot comparision
        fig, ax = plt.subplots(2, 1)

        ax[0].set_xlabel('t [s]')
        ax[0].set_ylabel('theta [°]')
        ax[0].plot(t_g, theta_g * 180 / pi, '-k', label='gen-al')
        ax[0].plot(t_m, theta_m * 180 / pi, '-r', label='moreau')
        ax[0].legend()

        ax[1].set_xlabel('t [s]')
        ax[1].set_ylabel('g_Ni [m]')
        ax[1].plot(t_g, g_N_g[:, 0], '-k', label='g_N1_g')
        ax[1].plot(t_g, g_N_g[:, 1], '--k', label='g_N2_g')
        ax[1].plot(t_m, g_N_m[:, 0], '-r', label='g_N1_m')
        ax[1].plot(t_m, g_N_m[:, 1], '--r', label='g_N2_m')
        ax[1].legend()

        plt.show()


import os
import sys

currentdir = os.path.dirname(os.path.realpath(__file__))
parentdir = os.path.dirname(currentdir)
sys.path.append(parentdir)

from utils import read

lines = read.read_puzzle(1)
cals_per_elf = read.group_int_lines(lines)

cal_sums = [sum(cals) for cals in cals_per_elf]

max_cal = max(cal_sums)

print(f"Most cals carried by an elf is: {max_cal}")

top3_cals = sorted(cal_sums, reverse=True)[:3]
cal_sum_top3 = sum(top3_cals)

print(f"Sum of top 3 cals carried by elfs is: {cal_sum_top3}")


import pandas as pd
import numpy as np

import hdbscan
from umap import UMAP
from sklearn.metrics import adjusted_rand_score, adjusted_mutual_info_score

from matplotlib.backends.backend_pdf import PdfPages
import matplotlib.pyplot as plt
import seaborn as sns

input_files = snakemake.input["FFT_tables"]
input_IDs = snakemake.params["input_IDs"]
outfile = snakemake.output["plots"]
out_metrics = snakemake.output["metrics"]

min_freq = snakemake.params["min"]
max_freq = snakemake.params["max"]
umap_epochs = snakemake.params["umap_epochs"]
umap_comps = snakemake.params["umap_comps"]
clust_size = snakemake.params["HDBSCAN_min_clustsize"]
metric_ID = snakemake.params["metric_ID"]

FFT_df = pd.DataFrame()
for ID,path in zip(input_IDs,input_files):
    indf=pd.read_csv(path,sep="\t",index_col="#Region")
    indf = indf.loc[:,(indf.columns.astype(int) >= int(min_freq)) & (indf.columns.astype(int) <= int(max_freq))]
    FFT_df[ID]=indf.values.flatten()

#conds = [FFT_df.T.index.str.contains("NPH")==True,
#        (FFT_df.T.index.str.contains("P") == True) & (FFT_df.T.index.str.contains("NPH") == False),
#        (FFT_df.T.index.str.contains("C") == True),
#        (FFT_df.T.index.str.contains("B") == True)]
#
#choices = ["healthy","P","C","B"]

conds = [ (FFT_df.T.index.str.contains("NPH")==True) ,
             ((FFT_df.T.index.str.contains("BH0")==True) | (FFT_df.T.index.str.contains("IH0")==True)),
        (FFT_df.T.index.str.contains("P") == True) & (FFT_df.T.index.str.contains("NPH") == False),
        (FFT_df.T.index.str.contains("C2") == True),
        (FFT_df.T.index.str.contains("B") == True) & (FFT_df.T.index.str.contains("BH") == False),
        (FFT_df.T.index.str.contains("IC") == True)   ]
choices = ["healthy","healthy_Cell","prostate","colon","breast","disease_Cell"]

conds_source = [((FFT_df.T.index.str.contains("BH0")==True) | (FFT_df.T.index.str.contains("IH0")==True)),
            (FFT_df.T.index.str.contains("IC")==True),
             (FFT_df.T.index.str.contains("NPH")==True),
            ((FFT_df.T.index.str.contains("C2_")==True) | (FFT_df.T.index.str.contains("P")==True) | (FFT_df.T.index.str.contains("NPH")==False)),
            ]
choices_source = ["healthy_Cell","disease_Cell","healthy_Graz","disease_Graz"]

conds_bin = conds_bin = [(FFT_df.T.index.str.contains("NPH")==True),
            ((FFT_df.T.index.str.contains("C2_")==True) | (FFT_df.T.index.str.contains("P")==True) | (FFT_df.T.index.str.contains("NPH")==False)),
            ((FFT_df.T.index.str.contains("BH0")==True) | (FFT_df.T.index.str.contains("IH0")==True)),
            (FFT_df.T.index.str.contains("IC")==True)
            ]
choices_bin = ["healthy_Graz","disease_Graz","healthy_Cell","disease_Cell"]

FFT_df_T = FFT_df.T
FFT_df_T["status"] = np.select(conds, choices)
FFT_df_T["status_bin"] = np.select(conds_bin, choices_bin)
FFT_df_T["status_source"] = np.select(conds_source, choices_source)

umap_2d = UMAP(n_components=2, n_epochs=300)
umap_projections = umap_2d.fit_transform(FFT_df.T)

# ground truth
projection = plt.figure(figsize=(15,8))
sns.scatterplot(x=umap_projections[:,0],
                y=umap_projections[:,1],
                hue=FFT_df_T["status"])
plt.title(f"UMAP 2D projection: FFT values ({min_freq}-{max_freq}bp) ground truth", fontsize=20)
plt.xlabel("UMAP_0")
plt.ylabel("UMAP_1")
plt.close()

projection_bin = plt.figure(figsize=(15,8))
sns.scatterplot(x=umap_projections[:,0],
                y=umap_projections[:,1],
                hue=FFT_df_T["status_bin"])
plt.title(f"UMAP 2D projection: FFT values ({min_freq}-{max_freq}bp) binary labels", fontsize=20)
plt.xlabel("UMAP_0")
plt.ylabel("UMAP_1")
plt.close()

projection_source = plt.figure(figsize=(15,8))
sns.scatterplot(x=umap_projections[:,0],
                y=umap_projections[:,1],
                hue=FFT_df_T["status_source"])
plt.title(f"UMAP 2D projection: FFT values ({min_freq}-{max_freq}bp) labels by dataset", fontsize=20)
plt.xlabel("UMAP_0")
plt.ylabel("UMAP_1")
plt.close()

# HDBSCAN on whole data
clusterer_all = hdbscan.HDBSCAN(min_cluster_size=clust_size)
cluster_all_labels = clusterer_all.fit_predict(FFT_df_T.iloc[:,:-3])

HDBSCAN_all_projection = plt.figure(figsize=(15,8))
sns.scatterplot(x=umap_projections[:,0],
                y=umap_projections[:,1],
                hue=cluster_all_labels.astype(str))
plt.title(f"UMAP 2D projection: FFT values ({min_freq}-{max_freq}bp) HDBSCAN no UMAP", fontsize=20)
plt.xlabel("UMAP_0")
plt.ylabel("UMAP_1")
plt.close()

# create metric df

clustered = (cluster_all_labels >= 0)

metric_dict = {"ID":[metric_ID],
            "raw_ARS":[adjusted_rand_score(FFT_df_T["status"], cluster_all_labels)],
            "noOutliers_ARS":[adjusted_rand_score(FFT_df_T["status"][clustered], cluster_all_labels[clustered])],
            "raw_AMI":[adjusted_mutual_info_score(FFT_df_T["status"], cluster_all_labels)],
            "noOutliers_AMI":[adjusted_mutual_info_score(FFT_df_T["status"][clustered], cluster_all_labels[clustered])],
            "raw_ARS_bin":[adjusted_rand_score(FFT_df_T["status_bin"], cluster_all_labels)],
            "noOutliers_ARS_bin":[adjusted_rand_score(FFT_df_T["status_bin"][clustered], cluster_all_labels[clustered])],
            "raw_AMI_bin":[adjusted_mutual_info_score(FFT_df_T["status_bin"], cluster_all_labels)],
            "noOutliers_AMI_bin":[adjusted_mutual_info_score(FFT_df_T["status_bin"][clustered], cluster_all_labels[clustered])],
            "raw_ARS_source":[adjusted_rand_score(FFT_df_T["status_source"], cluster_all_labels)],
            "noOutliers_ARS_source":[adjusted_rand_score(FFT_df_T["status_source"][clustered], cluster_all_labels[clustered])],
            "raw_AMI_source":[adjusted_mutual_info_score(FFT_df_T["status_source"], cluster_all_labels)],
            "noOutliers_AMI_source":[adjusted_mutual_info_score(FFT_df_T["status_source"][clustered], cluster_all_labels[clustered])]
           }

metric_df = pd.DataFrame(metric_dict)

metric_df.to_csv(out_metrics, sep="\t",index=None)

with PdfPages(outfile) as pdf:
    pdf.savefig(projection.get_figure())
    pdf.savefig(projection_bin.get_figure())
    pdf.savefig(projection_source.get_figure())
    pdf.savefig(HDBSCAN_all_projection.get_figure())


# coding: utf-8

import os
from datetime import datetime

################################################################################
'''Identify data!!!
In future, based on name of the file (eg.: VO_ZA_GUMTREE_2018_12.csv) we will know the identity of the data. For now, I manually set it:
'''
filename='IMMO_DE_IMMOWELT_2018_10.csv'
os.chdir(r'D:\19. ETL project\QC check')
#sys.path.insert(0,r'D:\19. ETL project\IMMO DE')

market, country, website, year, month = filename[:-4].split(sep='_')

#import pandas as pd
#data=pd.read_csv(filename, sep='\t',error_bad_lines = False,encoding='latin-1',dtype={'CP':str},nrows=100)

#Call the test for that data
from QualityCheck import validation
result, failed_tests = validation(filename,market,country,website)

################################################################################
#Send result to elasticsearch
from elasticsearch import Elasticsearch

#Connent to elasticsearch and send log
content = {
        'market': market,
        'country': country,
        'website': website,
        'validation': result['success'],
        'statistics': result['statistics'],
        'failed': failed_tests,
        'all_result': result['results'],
        'timestamp': datetime.now(),
    }
es=Elasticsearch('http://149.202.79.37:9200')
es.index(index=country+"_"+website,doc_type="validation", body=content)
################################################################################
#_search=es.search(index="za_cars")
#for i in es.indices.get("*"):
#    print (i)


import numpy as np
import cv2
from matplotlib import pyplot as plt
import glob
import os
from pathlib import Path
import numpy as np
import seaborn as sns
import pandas as pd

MIN_MATCH_COUNT = 10
nilai_list = []
img1 = cv2.imread('/home/agus/feature_match/query.jpg',0)#queryImage
folder = sorted(glob.glob('*.jpg'))
for img2 in folder:
    img2 = cv2.imread(img2,0)#trainImage
    # print(folder) #cek urutan file yang terbaca
    # Initiate SIFT detector
    sift = cv2.xfeatures2d.SIFT_create()

    # find the keypoints and descriptors with SIFT
    kp1, des1 = sift.detectAndCompute(img1,None)
    kp2, des2 = sift.detectAndCompute(img2,None)

    FLANN_INDEX_KDTREE = 0
    index_params = dict(algorithm = FLANN_INDEX_KDTREE, trees = 10)
    search_params = dict(checks = 50)

    flann = cv2.FlannBasedMatcher(index_params, search_params)

    matches = flann.knnMatch(des1,des2,k=2)

    # store all the good matches as per Lowe's ratio test.
    good = []
    for m,n in matches:
        if m.distance < 0.7*n.distance:
            good.append(m)

    if len(good)>MIN_MATCH_COUNT:
        src_pts = np.float32([ kp1[m.queryIdx].pt for m in good ]).reshape(-1,1,2)
        dst_pts = np.float32([ kp2[m.trainIdx].pt for m in good ]).reshape(-1,1,2)

        M, mask = cv2.findHomography(src_pts, dst_pts, cv2.RANSAC,5)
        matchesMask = mask.ravel().tolist()

        h,w = img1.shape
        pts = np.float32([ [0,0],[0,h-1],[w-1,h-1],[w-1,0] ]).reshape(-1,1,2)
        dst = cv2.perspectiveTransform(pts,M)

        img2 = cv2.polylines(img2,[np.int32(dst)],True,255,3, cv2.LINE_AA)
        #print(mask)
        inliers = np.sum(mask)
        matched = len(mask)
        value_FM = (inliers/matched)*100
        # print('%d / %d  inliers/matched' % (inliers,matched))
        print("akurasi: %f" %(value_FM))#print akurasi matching (inliers/matched)
        # print("precision: %f" %(1-((matched-inliers)/matched)))
    else:
        # print("Not enough matches are found : %d/%d" % (len(good),MIN_MATCH_COUNT)) #good keypoint/minimal keypoint
        matchesMask = None
        value_FM = 0
        print("akurasi: %f" %(value_FM))

    nilai_list.append(value_FM)

    draw_params = dict(matchColor = (0,255,0), # draw matches in green color
                    singlePointColor = None,
                    matchesMask = matchesMask, # draw only inliers
                    flags = 2)

    img3 = cv2.drawMatches(img1,kp1,img2,kp2,good,None,**draw_params)

#list program untuk menemukan gambar yang cocok dengan query image
match_img = nilai_list.index(max(nilai_list))
kondisi = np.count_nonzero(nilai_list)

if kondisi == 0 :
    print("tidak ada yang cocok")
else:
    print(match_img+1)
    match_img = str(match_img+1)+'.jpg'
    # print(type(match_img))
    cocok = cv2.imread(match_img,0)
    # print(type(cocok))
    plt.imshow(cocok,'gray') #plot gambar yang terbaik nilai presisinya
    # plt.plot(nilai_list) untuk plot grafik
    # plt.ylabel('nilai')
    plt.show()


from torchtext import data

TEXT = data.Field()
LABELS = data.Field()

train, val, test = data.TabularDataset.splits(
    path='~/chainer-research/jmt-data/pos_wsj/pos_wsj', train='.train',
    validation='.dev', test='.test', format='tsv',
    fields=[('text', TEXT), ('labels', LABELS)])

print(train.fields)
print(len(train))
print(vars(train[0]))

train_iter, val_iter, test_iter = data.BucketIterator.splits(
    (train, val, test), batch_size=3, sort_key=lambda x: len(x.text), device=0)

LABELS.build_vocab(train.labels)
TEXT.build_vocab(train.text)

print(TEXT.vocab.freqs.most_common(10))
print(LABELS.vocab.itos)

batch = next(iter(train_iter))
print(batch.text)
print(batch.labels)


import ujson
import logging
from contextlib import closing

from gevent.threading import Lock
from pysolmeters.Meters import Meters

from pysolmysql.Pool.mysql_pool import MysqlConnectionPool

logger = logging.getLogger(__name__)

class MysqlApi(object):
    """
    Mysql Api
    """

    # Lock
    POOL_LOCK = Lock()

    # Static pool instances (hash from config dict => MysqlConnectionPool)
    D_POOL_INSTANCES = dict()

    @classmethod
    def reset_pools(cls):
        """
        Reset all pools
        """

        with cls.POOL_LOCK:
            for s_hash, pool in cls.D_POOL_INSTANCES.items():
                logger.info("Closing pool, s_hash=%s", s_hash)
                pool.close_all()
            cls.D_POOL_INSTANCES = dict()

    @classmethod
    def _get_pool_hash(cls, conf_dict):
        """
        Get pool hash
        :param conf_dict: dict
        :type conf_dict: dict
        :return: str
        :rtype: str
        """

        s_hash = str(hash(ujson.dumps(conf_dict, sort_keys=True)))
        return s_hash

    @classmethod
    def _get_pool(cls, conf_dict):
        """
        Init static pool
        :param conf_dict: dict
        :type conf_dict: dict
        :return pysolmysql.Pool.mysql_pool.MysqlConnectionPool
        :rtype pysolmysql.Pool.mysql_pool.MysqlConnectionPool
        """

        # Hash
        s_hash = cls._get_pool_hash(conf_dict)

        # Alloc if needed
        if s_hash not in cls.D_POOL_INSTANCES:
            with cls.POOL_LOCK:
                if s_hash not in cls.D_POOL_INSTANCES:
                    cls.D_POOL_INSTANCES[s_hash] = MysqlConnectionPool(conf_dict)
                    logger.info("Allocated pool, s_hash=%s, pool.len=%s", s_hash, len(cls.D_POOL_INSTANCES))
                    Meters.aii("k.db_pool.hash.cur")

        # Over
        return cls.D_POOL_INSTANCES[s_hash]

    @classmethod
    def _fix_type(cls, data):
        """
        Fix type
        :param data: data
        """
        if isinstance(data, bytearray):
            return data.decode("utf-8")
        else:
            return data

    @classmethod
    def exec_0(cls, conf_dict, statement):
        """
        Execute a sql statement, returning row affected.
        :param conf_dict: configuration dict
        :type conf_dict: dict
        :param statement: statement to execute
        :type statement: str
        :rtype: int
        :return rows affected
        """

        cnx = None
        try:
            cnx = cls._get_pool(conf_dict).connection_acquire()
            with closing(cnx.cursor()) as cur:
                cur.execute(statement)
                return cur.rowcount
        finally:
            cls._get_pool(conf_dict).connection_release(cnx)

    @classmethod
    def exec_n(cls, conf_dict, statement, fix_types=True):
        """
        Execute a sql statement, returning 0..N rows
        :param conf_dict: configuration dict
        :type conf_dict: dict
        :param statement: statement to execute
        :type statement: str
        :param fix_types: If true, fix data type
        :type fix_types: bool
        :return list of dict.
        :rtype list
        """

        cnx = None
        try:
            cnx = cls._get_pool(conf_dict).connection_acquire()
            with closing(cnx.cursor()) as cur:
                cur.execute(statement)
                rows = cur.fetchall()
                for row in rows:
                    logger.debug("row=%s", row)
                    for k, v in row.items():
                        logger.debug("k=%s, %s, %s", k, type(v), v)
                        if fix_types:
                            row[k] = MysqlApi._fix_type(v)
                return rows
        finally:
            cls._get_pool(conf_dict).connection_release(cnx)

    @classmethod
    def exec_1(cls, conf_dict, statement, fix_types=True):
        """
        Execute a sql statement, returning 1 row.
        Method will fail if 1 row is not returned.
        :rtype: object
        :param conf_dict: configuration dict
        :type conf_dict: dict
        :param statement: statement to execute
        :type statement: str
        :param fix_types: If true, fix data type
        :type fix_types: bool
        :return dict
        :rtype dict
        """

        cnx = None
        try:
            cnx = cls._get_pool(conf_dict).connection_acquire()
            with closing(cnx.cursor()) as cur:
                cur.execute(statement)
                rows = cur.fetchall()
                for row in rows:
                    logger.debug("row=%s", row)
                    for k, v in row.items():
                        logger.debug("k=%s, %s, %s", k, type(v), v)
                        if fix_types:
                            row[k] = MysqlApi._fix_type(v)
                if len(rows) != 1:
                    raise Exception("Invalid row len, expecting 1, having={0}".format(len(rows)))
                return rows[0]
        finally:
            cls._get_pool(conf_dict).connection_release(cnx)

    @classmethod
    def exec_01(cls, conf_dict, statement, fix_types=True):
        """
        Execute a sql statement, returning 0 or 1 row.
        Method will fail if 0 or 1 row is not returned.
        :param conf_dict: configuration dict
        :type conf_dict: dict
        :param statement: statement to execute
        :type statement: str
        :param fix_types: If true, fix data type
        :type fix_types: bool
        :return dict, None
        :rtype dict, None
        """

        cnx = None
        try:
            cnx = cls._get_pool(conf_dict).connection_acquire()
            with closing(cnx.cursor()) as cur:
                cur.execute(statement)
                rows = cur.fetchall()
                for row in rows:
                    logger.debug("row=%s", row)
                    for k, v in row.items():
                        logger.debug("k=%s, %s, %s", k, type(v), v)
                        if fix_types:
                            row[k] = MysqlApi._fix_type(v)
                if len(rows) == 0:
                    return None
                elif len(rows) != 1:
                    raise Exception("Invalid row len, expecting 1, having={0}".format(len(rows)))
                else:
                    return rows[0]
        finally:
            cls._get_pool(conf_dict).connection_release(cnx)

    @classmethod
    def multi_n(cls, conf_dict, ar_statement):
        """
        Execute multiple sql statement, reading nothing from mysql.
        :type conf_dict: dict
        :param ar_statement: list of statements to execute (for instance, batch of insert or whatever)
        :type ar_statement: list
        """

        cnx = None
        try:
            cnx = cls._get_pool(conf_dict).connection_acquire()
            with closing(cnx.cursor()) as cur:
                for s in ar_statement:
                    cur.execute(s)
        finally:
            cls._get_pool(conf_dict).connection_release(cnx)


"""
## Name of Prob
B. Game with Telephone Numbers

## Link
https://codeforces.com/contest/1155/problem/B

## Note
A telephone number is a sequence of exactly 11 digits such that its first digit is 8.

## Input

## Output

## Strategy
there is (n - 12) turns to determine
after (n - 12) turns, if there must be 88[0-9]{12} then Vasya win

Petya will move to remove every 8

count[8] < (n - 12) // 2 => NO because he will remove all 8's

Petya will remove leading 8's
Vasya remove not 8 letter from first

count number of 8's until found (turn + 1) // 2 not '8' s
"""
from collections import Counter

_DEBUG = True

def solve(n, s):
    c = Counter(s)
    turn_count = n - 12
    if c['8'] < turn_count // 2:
        return 'NO'

    v_count = 0
    eight_count = 0
    for i in range(n):
        if s[i] != '8':
            v_count += 1
            if v_count >= (turn_count + 1) // 2:
                j = i + 1
                while j < n and s[j] == '8':
                    eight_count += 1
                    j += 1
                break
        else:
            eight_count += 1
    # print(eight_count, turn_count)
    if eight_count >= turn_count // 2 + 2:
        return 'YES'
    return 'NO'

assert solve(13, '8380011223344') == 'YES'
assert solve(15, '807345619350641') == 'NO'
assert solve(29, '88811188118181818118111111111') == 'NO'
assert solve(19, '8181818181111111111') == 'YES'
assert solve(13, '9999999998888') == 'NO'
assert solve(13, '0000008888888') == 'NO'
assert solve(13, '2480011223348') == 'NO'
assert solve(17, '87879887989788999') == 'YES'

n = int(input())
s = input()
solution = solve(n, s)
print(solution)

codeforces/contests/1155/B. Game with Telephone Numbers.py