from .region import BaseRegion
from .province import BaseProvince
from datetime import datetime
import requests
from bs4 import BeautifulSoup
from html_table_extractor.extractor import Extractor
import re
from statistics import mean

class EmiliaRomagna(BaseRegion):
    """
    Implementation of Emilia-Romagna
    """
    name = "Emilia-Romagna"

    def __init__(self):
        super().__init__()

        # adding provinces
        self.add_province(BaseProvince(name='Bologna', short_name='BO'))
        self.add_province(BaseProvince(name='Rimini', short_name='RN'))
        self.add_province(BaseProvince(name='Ferrara', short_name='FE'))
        self.add_province(BaseProvince(name='Forlì-Cesena', short_name='FC'))
        self.add_province(BaseProvince(name='Modena', short_name='MO'))
        self.add_province(BaseProvince(name='Parma', short_name='PR'))
        self.add_province(BaseProvince(name='Piacenza', short_name='PC'))
        self.add_province(BaseProvince(name='Ravenna', short_name='RA'))
        self.add_province(BaseProvince(name='Reggio Emilia', short_name='RE'))

    def extract_float(self, s: str) -> float:
        """
        Extract the first float from a string

        :param s: The string where the float will be extracted
        :return: The float, if any found, or None
        """
        f = re.findall(r'([0-9]*[.]*[0-9]+)', s)
        return float(f[0]) if len(f) > 0 else None

    def _fetch_air_quality_routine(self, day: datetime):
        """
        Populate the air quality of the provinces.
        Data is fetched from https://www.arpae.it/qualita-aria/bollettino-qa/{date} where {date}
        is the date of interest in the format YYYYMMDD

        :param day: The day of which the air quality wants to be known (instance of `~datetime`)
        """
        super()._fetch_air_quality_routine(day)

        date_fmt = day.strftime('%Y%m%d')
        res = requests.get(f'https://www.arpae.it/qualita-aria/bollettino-qa/{date_fmt}')

        if res.status_code == 200:
            soup = BeautifulSoup(res.text, 'html.parser')
            table_rows = '\n'.join([str(x) for x in soup.select('.tabella table tbody tr')])
            big_table = f'<table>{table_rows}</table>'
            extractor = Extractor(big_table)
            extractor.parse()
            table = extractor.return_list()

            for province in self.provinces:
                province_rows = [x for x in table if x[0] == province.short_name]

                so2 = [self.extract_float(x[9]) for x in province_rows if self.extract_float(x[9]) is not None]
                no2 = [self.extract_float(x[4]) for x in province_rows if self.extract_float(x[4]) is not None]
                co = [self.extract_float(x[8]) for x in province_rows if self.extract_float(x[8]) is not None]
                pm10 = [self.extract_float(x[2]) for x in province_rows if self.extract_float(x[2]) is not None]
                pm25 = [self.extract_float(x[3]) for x in province_rows if self.extract_float(x[3]) is not None]
                o3 = [self.extract_float(x[6]) for x in province_rows if self.extract_float(x[6]) is not None]
                c6h6 = [self.extract_float(x[7]) for x in province_rows if self.extract_float(x[7]) is not None]

                if len(so2) > 0: province.quality.so2 = round(mean(so2), 2)
                if len(no2) > 0: province.quality.no2 = round(mean(no2), 2)
                if len(co) > 0: province.quality.co = round(mean(co), 2)
                if len(pm10) > 0: province.quality.pm10 = round(mean(pm10), 2)
                if len(pm25) > 0: province.quality.pm25 = round(mean(pm25), 2)
                if len(o3) > 0: province.quality.o3 = round(mean(o3), 2)
                if len(c6h6) > 0: province.quality.c6h6 = round(mean(c6h6), 2)

        if self.on_quality_fetched is not None: self.on_quality_fetched(self)


#!/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 json
from difflib import get_close_matches
data=json.load(open("data.json"))

def A(w):
    w=w.lower()
    if w in data:
        return data[w]
    elif len(get_close_matches(w,data.keys()))>0 :
        yn=input("Doyoumean %s  ?.Type Y/N:" % get_close_matches(w,data.keys())[0])
        if yn=="Y":
            return data[get_close_matches(w,data.keys())[0]]
        elif yn=="N":
            return ("Word not exist")

            if Q in data:
                return data[w]

        else:
            return ("Word does not exit")

    else:
        return ("safasf")

word=input("Enter Word: ")

output = A(word)

if type(output)==list:

    for P in output:
        print(P)
else:

    print(output)


#!/usr/bin/env python2
import rospy
import tf_conversions
from geometry_msgs.msg import PoseStamped

goal_theta = 0.0

def goal_publisher():
    pub = rospy.Publisher('move_base_simple/goal', PoseStamped, queue_size=1)
    rospy.init_node('goal_pub', anonymous=True)
    rate = rospy.Rate(60)
    global theta
    ct = 0

    while ct <1000 and(not rospy.is_shutdown()):
        goal = PoseStamped();
        ct+=1
        goal.header.stamp = rospy.Time.now()
        goal.header.frame_id = "odom"

        goal.pose.position.x = 10.0
        goal.pose.position.y = 5.5
        goal.pose.position.z = 0.0

        goal_quat = tf_conversions.transformations.quaternion_from_euler(0, 0, goal_theta)

        goal.pose.orientation.x = goal_quat[0]
        goal.pose.orientation.y = goal_quat[1]
        goal.pose.orientation.z = goal_quat[2]
        goal.pose.orientation.w = goal_quat[3]

        pub.publish(goal)

        rate.sleep()

if __name__ == '__main__':
    try:
        goal_publisher()
    except rospy.ROSInterruptException:
        pass


from sqlite3 import connect, OperationalError, ProgrammingError
from sqlite3 import Connection, Cursor
from pathlib import Path
from typing import Generator, Tuple, Pattern, List
from ..babelio.babelfiler import BabelFiler
from distutils.util import strtobool
from collections import defaultdict
from re import compile
from ..babeldata.babellangcode import BabelLangCode
from collections import namedtuple

class BabelDB:
    """General db object methods"""

    def __init__(self, db_name: str = None, db_connect: object = None) -> None:
        self.db_name = db_name
        self.db_connect = db_connect
        if not self.db_name:
            self.db_name = "babeldb"

        if not self.db_connect:
            self.db_connect = ConnectDB(database=self.db_name)

    def __repr__(self):
        return f"BabelDB('test')"

    def __str__(self):
        return f"class BabelDB.Methods([GET]) from Local " \
               f"Database."

    #  An attempt to disconnect properly if class is instantiate using with.
    def __enter__(self):
        return self

    def __exit__(self, exc_type: type, db_connect, exc_tb):
        return self.db_connect.close_connection()

    @property
    def db_name(self):
        return self._dbname

    @db_name.setter
    def db_name(self, name):
        name, *junk = name.rsplit(".")
        del junk
        while True:
            t_name = f"{name}.db"

            try:
                connect(f"file:{Path(t_name).absolute()}?mode=rw", uri=True)

            except (OperationalError, ProgrammingError) as error:
                print(f"DB-file does not exist 'db_name' -> {error}")
                user_choice = strtobool(input(f"Do you wish to create db "
                                              f"with name: {t_name}?: Y/N"))
                if not user_choice:
                    name = input("Input absolute path or relative path "
                                 "local or remote database file here:\n")
                    if name == "exit":
                        self._dbname = None
                        break
                elif user_choice:
                    BabelFiler.babel_mkfile(new_file=name, suffix=".db")
                    print(f"File is created {name}")
                    self._dbname = f"{name}.db"
                    break

            else:
                print(f"db is loaded -> {name}.db")
                self._dbname = f"{name}.db"
                break

    def _create_langcode_table(self):
        conn = self.db_connect
        cursor = conn.cursor()
        cursor.execute('''CREATE TABLE IF NOT EXISTS lang_codes
        (lang_id INTEGER PRIMARY KEY NOT NULL,
        date_added DATE NOT NULL,
        comments VARCHAR(80),
        deprecated DATE,
        description TEXT,
        macrolanguage VARCHAR(80),
        preferred_value VARCHAR(40),
        prefix VARCHAR(20),
        scope VARCHAR(24),
        subtag VARCHAR(24),
        suppress_script VARCHAR(50),
        tag VARCHAR(40),
        type VARCHAR(60)
        )''')
        conn.commit()

    def _json_to_tuples(self, json_obj: dict) -> list:
        values = list()
        langcode_obj = BabelLangCode()
        langcode_obj.lang_columns = json_obj
        d1 = langcode_obj.lang_columns.copy()
        json_obj = self._jsondata_filtered(json_obj=json_obj)

        for i, v in enumerate(json_obj):
            d1.update({"Index": i})
            d1.update(v)
            values.append(tuple(d1.values()))
            d1 = langcode_obj.lang_columns.copy()
        return values

    def _jsondata_filtered(self, json_obj: dict) -> Generator[dict, None,
                                                              None]:
        return (json_obj.get(i) for i in json_obj if json_obj[i]["Type"] ==
                "language")

    def load_langcode_data(self, json_obj):
        self._create_langcode_table()
        values = self._json_to_tuples(json_obj=json_obj)
        conn = self.db_connect
        cursor = conn.cursor()
        cursor.executemany('''INSERT INTO lang_codes(
        lang_id, date_added, comments, deprecated,
        description, macrolanguage, preferred_value, prefix,
        scope, subtag, suppress_script, tag, type) VALUES
        (?,?,?,?,?,?,?,?,?,?,?,?,?)''', values)
        conn.commit()
        self._create_look_up()

    def _create_look_up(self):
        conn = self.db_connect
        cursor = conn.cursor()
        cursor.execute("""
        CREATE TABLE look_up as SELECT lang_id, description, subtag
        FROM lang_codes
        """)
        conn.commit()

    def get_langcode(self, language: Tuple[str, str]):
        """
        Method returns language codes from local database. Data is fetched
        with or statement i.e.similar to this a=n or b=m.

        language : tuple(ISO standard country name: str, RFC3066-tag)

        """
        conn = self.db_connect
        cursor = conn.cursor()
        try:
            assert all(map(lambda x: isinstance(x, str), language)) is True, \
                "Input must be str!"
            assert len(language) == 2, "'language' input must be of length 2."
            if not language[0].isalpha():
                language = ("!", language[1])  # ! function as an esc-character

            cursor.execute("SELECT * FROM look_up WHERE description LIKE "
                           "? OR subtag=? LIMIT 20", ('%'+language[0]+'%',
                                                      language[1])
                           )
        except ProgrammingError as error:
            print(f"DB has been error triggered:\n{error}")
            return

        except AssertionError as error:
            print(error)

        else:
            found_in_db = cursor.fetchall()
            conn.commit()
            if not found_in_db:
                warning = UserWarning("DB found no matches.")
                print(warning)
                return None, None

            result = BabelDB.select_lang_code(found_in_db)
            return result

    def _get_column_labels(self, cursor: Cursor, target: Pattern[str] = None) \
            -> \
            list:
        if not target:
            return [i for i in map(lambda x: x[0], cursor.description) if i]
        else:
            pattern = compile(target)
            return [i for i in map(lambda x:x[0], cursor.description) if
                    pattern.fullmatch(i)]

    def pptable(self, column_width: int = 24, nr_rows: int = 5) -> str:
        conn = self.db_connect
        col_w = column_width
        cursor = conn.cursor()
        cursor.execute('SELECT * FROM lang_codes')
        headers = self._get_column_labels(cursor,)
        col_names = BabelDB.parse_table(table_data=headers,
                                        columnwidth=col_w, sep="#")
        cursor.execute('SELECT * FROM lang_codes')
        print(col_names)
        data = cursor.fetchall()
        for i, v in enumerate(data, start=1):
            row = BabelDB.parse_table(table_data=v, columnwidth=col_w)
            if i <= nr_rows:
                print(row)
            else:
                break

        return "Done!"

    @staticmethod
    def select_lang_code(db_values: List[tuple] = None):
        LangCode = namedtuple("LangCode", ["lang_id", "iso_standard_name",
                                           "subtag"], defaults=((None, ) * 3))
        user_selected = LangCode()
        option_dict = {}

        print(">>> Pick language by entering a digit (see below)")
        for i, v in enumerate(db_values, 1):
            option_dict.update({i: v})
            print(f"Option - {i}: {v}")
        while True:
            user_picked = input("Please enter a digit: ")
            try:
                user_picked = int(user_picked)
            except (ValueError, TypeError):
                print(f"An invalid entry was passed: {user_picked}")
                continue

            else:
                if option_dict.get(user_picked):
                    return user_selected._make(option_dict.get(user_picked))
                else:
                    print("Try Again!")

    @staticmethod
    def parse_table(table_data, columnwidth, sep: str = "|"):
        table = defaultdict(str)
        for i, rows in enumerate(table_data):
            cell = BabelDB._width_calculator(rows, column_width=columnwidth,
                                             sep=sep)
            table[i] = "".join([table[i], "".join(list(cell))])
        return "".join([table.get(k) for k in table.keys()])

    @staticmethod
    def _width_calculator(
            word: str, column_width: int = 24, spacer: int = 2, sep: str = "|"
    ) -> Generator[str, None, None]:

        word = str(word)

        if column_width <= len(word):
            word = word[(spacer*4*-1):]

        if len(word) % 2:
            word = "".join([word, " "])

        try:
            assert isinstance(word, str)
            empty_space = int(((column_width - (spacer * 2)) - len(word)) *
                              0.5)
            assert(empty_space >= 0)

        except (AssertionError, TypeError):
            width_factor = 2
            word = f"{word}"
            yield f"{width_factor * ' '}{word}{width_factor * ' '}{sep}"

        else:
            yield f"{empty_space *' '}{word}{empty_space * ' '}{sep}"

class ConnectDB(Connection):
    def __init__(self, database: str = None, is_db_connected: bool = True):
        super(ConnectDB, self).__init__(database=database)
        self.is_connected = is_db_connected
        self.database = database

    def __str__(self):
        return "ConnectDB(sqlite3.Connect)"

    def __repr__(self):
        return "demo_db = ConnectDb(database='demo_db.db')"

    @property
    def is_connected(self):
        return self._connection

    @is_connected.setter
    def is_connected(self, state):
        self._connection = state

    def close_connection(self):
        self.is_connected = False
        cursor = super().cursor()
        cursor.close()
        return super().close()


from __future__ import unicode_literals
import frappe

def execute(filters=None):
	#Item Code	Asset Name	Asset Catergory	Warehouse	Purchase Date	Value (After Depreciation)
	columns, data = ["Item Code:Link/Item:150","Asset Name::150","Asset Category:Link/Asset Category:150","Warehouse:Link/Warehouse:150","Purchase Date:Date:150","Book Value:Currency:200","Gross Purchase Amount:Currency:200"], []
	where=""
	if filters.get("asset_category"):
		where = """ and asset_category = "{}" """.format(filters.get("asset_category"))
	data = frappe.db.sql("""select item_code,name,asset_category,warehouse,purchase_date,value_after_depreciation,gross_purchase_amount
		from tabAsset where docstatus=1 {}""".format(where),as_list=1)
	return columns, data


import torch
from torch.testing import assert_close
import unittest

from scaled_dot_product import ScaledDotProductAttention

class TestScaledDotProductAttention(unittest.TestCase):

	def test_single_attention(self):
		module = ScaledDotProductAttention()
		query = torch.Tensor([[0, 3, 0, 0]])
		keys = torch.Tensor([[
			[0, 3, 0, 0],
			[0, 0, 0, 0],
			[0, 0, 0, 3]
		]])
		values = torch.Tensor([[
			[1, 2, 3],
			[9, 8, 7],
			[1, 8, 3]
		]])

		# Beacuse the query exactly matches the first key,
		# the module should approximatelty compute the first
		# value.
		assert_close(module(query, keys, values), torch.tensor([[[1.0, 2.0, 3.0]]]), atol=.2, rtol=0)

if __name__ == '__main__':
    unittest.main()


import random
from scipy import array as ar
from polls.question_generators.shape.shape_tools import *
from polls.question_generators.tools import *
import math

def gen_rectangle_area(a1,a2,a3,a4,a5,a6):
    clear_temp_img()
    questions = []
    answers = []
    count = [i for i in range(8)]

    for i in count:
        if i == 4:
            q, a = rectangle_area_perimeter(0, random.randint(10,60))
        elif i >= 5:
            q,a = rectangle_area_perimeter(0, random.randint(280,340))
        else:
            q,a = rectangle_area_perimeter(0)
        questions.append(q)
        answers.append(a)

    return {'questions': questions, 'answers': answers, 'count': count}

def gen_rectangle_perimeter(a1,a2,a3,a4,a5,a6):
    clear_temp_img()
    questions = []
    answers = []
    count = [i for i in range(8)]

    for i in count:
        if i < 4:
            q,a = rectangle_area_perimeter(1, 0)
        else:
            q,a = rectangle_area_perimeter(1,random.randint(10,170))
        questions.append(q)
        answers.append(a)

    return {'questions': questions, 'answers': answers, 'count': count}

def rectangle_area_perimeter(area_or_perimeter, rotate_shape = 0):
    w = random.randint(3,10)
    l = random.randint(w+1,w+10)
    units = random.choice(['mm', 'cm', 'm'])

    points = ar([[0,0],[l,0],[l,w],[0,w], [0,0]])

    if rotate_shape != 0:
        points = rotate(points, rotate_shape)

    label_points = labels_for_shape(points)
    filler = ''
    for i in range(len(str(w))+len(units)):
        filler = filler + ' '

    labels = [str(l) + units, filler + str(w) + units, '', '']
    angs = []

    for j in range(len(points) - 1):
        if points[j + 1][0] == points[j][0]:
            angs.append(90)
        elif points[j + 1][1] == points[j][1]:
            angs.append(0)
        else:
            m = (points[j + 1][1] - points[j][1]) / (points[j + 1][0] - points[j][0])
            angs.append(math.degrees(math.atan(m)))

    fig = plot_shape(points, label_points, labels, 3, 0, angs)
    #if area_or_perimeter == 0:
    #    plt.text(l / 2, w / 2, 'Area = ?', horizontalalignment='center', verticalalignment='center', fontsize='large')
    #else:
    #    plt.text(l / 2, w / 2, 'Perimeter = ?', horizontalalignment='center', verticalalignment='center', fontsize='large')

    r = random.randint(0, 999999999999999)
    fn = 'temp_img/temp' + str(r) + '.png'

    fig.savefig('media/' + fn, pad_inches=0, dpi=200, transparent=True)
    question = fn

    if area_or_perimeter == 0:
        ans = '$' + str( l * w ) + units + '^2$'
    else:
        ans = '$' + str(l + l + w + w) + units + '$'

    return question, ans


from operator import itemgetter

def solve(n, s):
    s = enumerate(s)
    s = sorted(s, key=itemgetter(1))
    schedule = [None] * n
    end_time = [0, 0]
    for idx, time in s:
        begin, end = time
        if begin < end_time[0] and begin < end_time[1]:
            return "IMPOSSIBLE"
        if begin >= end_time[0]:
            end_time[0] = end
            schedule[idx] = 'C'
        elif begin >= end_time[1]:
            end_time[1] = end
            schedule[idx] = 'J'
    return ''.join(schedule)

for t in range(1, int(input()) + 1):
    n = int(input())
    s = [list(map(int, input().split())) for _ in range(n)]
    r = solve(n, s)
    print('Case #{}: {}'.format(t, r))


#!/usr/bin/env python3
from bs4 import BeautifulSoup
import json

class HtmlParser(object):
    #获取指定页面符合要求的内容
    def _parse_data(self,url,content,html_encode="utf-8"):
        if content is None:
            return
        soup=BeautifulSoup(content,"html.parser",from_encoding=html_encode)
        new_data=self._get_new_data(url,soup)
        return new_data
        # 获取数据
    def _get_new_data(self, url, soup):
        try:
            content=soup.find('script',id='__NEXT_DATA__').get_text().strip()
            jsonContent=json.loads(content)
            companies=jsonContent['props']['pageProps']['companies']
            return companies
        except  Exception as e:
            print(e)


# Subtracting elements from one array and adding them to another one (sorting elements of numerous arrays)

def mymax(y):
    yl = len(y)
    i = 0
    max = 0
    imax = 0
    # go over all elements of array and find imax (pos. of the maximum)
    while i < yl:
        test = y[i]
        if max < test:
            # new max found
            max = test
            imax = i
        i = i+1
    return imax

x1 = [34, 45, 56, 99999, -9, 101, 34, 555, -12,1,2,3,4,5,6,7,32,52,42,533453,4534,5,353,6,36,34,63,]
y1 = []
print("starting-------------------->")
i = len(x1)
while i > 0:
    J = mymax(x1)
    print("before", x1)
    print("Terminated", J, x1[J])
    y1.append(x1[J])
    x1.pop(J)
    print("after", x1)

    print("Newborn array", y1)
    i = i-1


import math
from ast import literal_eval

# from keras.layers import *
# from keras.layers.advanced_activations import LeakyReLU
# from keras.models import Model
import os
import time
from PIL import Image
import numpy as np
import tensorflow as tf
import tensorflow_hub as hub
import matplotlib.pyplot as plt

#from ds_utils import image_processing

def conv(filters, kernel_size=5, strides=2, leaky_relu=False):
    def block(x):
        x = Conv2D(filters, kernel_size=kernel_size, strides=strides,
                   padding='same')(x)
        x = BatchNormalization()(x)
        if leaky_relu:
            x = LeakyReLU(0.2)(x)
        else:
            x = Activation("relu")(x)
        return x
    return block

def res_block(filters, kernel_size=3):
    def block(input_tensor):
        x = conv(filters, kernel_size=kernel_size, strides=1, leaky_relu=True)(input_tensor)
        x = conv(filters, kernel_size=kernel_size, strides=1, leaky_relu=True)(input_tensor)
        x = Add()([x, input_tensor])
        x = LeakyReLU(alpha=0.2)(x)
        return x
    return block

def upscale(filters, kernel_size=3):
    def block(x):
        x = UpSampling2D()(x)
        x = Conv2D(filters, kernel_size=kernel_size, padding='same')(x)
        x = LeakyReLU(0.1)(x)
        return x
    return block

def get_upsampling_model(input_shape, upscale_factor=2):
    # get number of upscale blocks needed based on given factor
    # (2^upscale_blocks = upscale_factor)
    upscale_blocks = int(math.log(upscale_factor, 2))

    model_input = Input(shape=input_shape)
    x = model_input

    res_input_0 = conv(64, 3, 1)(x)

    x = res_block(64)(res_input_0)

    nb_residual = 5
    for i in range(nb_residual):
        x = res_block(64)(x)

    x = Add()([x, res_input_0])

    # if more should get input for Nth conv block and add for Nth res block
    for i in range(upscale_blocks):
        x = upscale(64, 3)(x)
        # first res_input is just model_input
        # x = Add()([x, res_inputs[i+2]])

    outp = Conv2D(3, 3, activation='linear', padding='same')(x)

    return model_input, outp

def get_SRResNet(cfg):
    models_path = cfg.get('models_path', None)
    LR_IMG_SHAPE = literal_eval(cfg.get('LR_IMG_SHAPE'))

    upsampled_inp, upsampled_output = get_upsampling_model(LR_IMG_SHAPE, 2)

    sr_model = Model(upsampled_inp, upsampled_output)
    #adam = Adam(lr=1e-3)
    #sr_model.compile(optimizer=adam, loss=loss_wrapper(vgg_hr, vgg_upsamples), metrics=[PSNRLoss])

    if models_path:
        print("Loading Super Resolution Models...")
        sr_model.load_weights(models_path + '/sr_model.h5')
        print("Super Resolution Models Loaded")

    return sr_model

################
# Training Utils
################
def train_generator(imgs_filepaths, img_size=None, batch_size=64, apply_fun=None):
    while True:
        imgs_paths_sample = np.random.choice(imgs_filepaths, batch_size)
        original_data = image_processing.load_data(imgs_paths_sample, img_size)
        original_data = original_data / 255.0
        if apply_fun:
            data = apply_fun(original_data)
        else:
            data = original_data
        yield data, original_data

def preprocess_image(image_path):
  """ Loads image from path and preprocesses to make it model ready
      Args:
        image_path: Path to the image file
  """
  hr_image = tf.image.decode_image(tf.io.read_file(image_path))
  # If PNG, remove the alpha channel. The model only supports
  # images with 3 color channels.
  if hr_image.shape[-1] == 4:
    hr_image = hr_image[...,:-1]
  hr_size = (tf.convert_to_tensor(hr_image.shape[:-1]) // 4) * 4
  hr_image = tf.image.crop_to_bounding_box(hr_image, 0, 0, hr_size[0], hr_size[1])
  hr_image = tf.cast(hr_image, tf.float32)
  return tf.expand_dims(hr_image, 0)

hr_image = preprocess_image("C:/Users/User/Documents/generated_data/sr/test/1.png")
SAVED_MODEL_PATH = "https://tfhub.dev/captain-pool/esrgan-tf2/1"
model = hub.load(SAVED_MODEL_PATH)
fake_image = model(hr_image)
image = tf.squeeze(fake_image)
if not isinstance(image, Image.Image):
    image = tf.clip_by_value(image, 0, 255)
    image = Image.fromarray(tf.cast(image, tf.uint8).numpy())
image.save("C:/Users/User/Documents/generated_data/sr/out.png")


# 傅立叶变换(np.fft)

# import matplotlib.pyplot as plt
# import numpy as np

# x = np.linspace(0,2*np.pi,30)#创建一个包含30个点的余弦波信号
# wave = np.cos(x)
## 傅立叶变换 ###
# transformed = np.fft.fft(wave)
## 逆傅立叶变换 ###
# print(np.all(np.abs(np.fft.ifft(transformed)-wave)<10**(-9)))#对变换后的结果应用ifft函数，应该可以近似地还原初始信号
# plt.plot(x,transformed.real)              # 注意，fft变换结果是复数，要取实部

# x = np.linspace(0,2*np.pi,30)
# wave = np.cos(x)#创建一个包含30个点的余弦波信号
# transformed = np.fft.fft(wave)#使用fft函数对余弦波信号进行傅里叶变换
## 频移 ###
# shifted = np.fft.fftshift(transformed)#使用fftshift函数进行移频操作。
## 逆频移
# print(np.all((np.fft.ifftshift(shifted)-transformed)<10**(-9))) #用ifftshift函数进行逆操作，这将还原移频操作前的信号。
# plt.plot(x,shifted.real)
# plt.show()

# 移频
# numpy.fft模块中的fftshift函数可以将FFT输出中的直流分量移动到频谱的中央。ifftshift函数则是其逆操作。
import numpy as np
from matplotlib.pyplot import plot, show
x = np.linspace(0, 2 * np.pi, 30)
wave = np.cos(x)  #创建一个包含30个点的余弦波信号。
transformed = np.fft.fft(wave)  #使用fft函数对余弦波信号进行傅里叶变换。
shifted = np.fft.fftshift(transformed) #使用fftshift函数进行移频操作。
print (np.all((np.fft.ifftshift(shifted) - transformed) < 10 ** -9))  #用ifftshift函数进行逆操作，这将还原移频操作前的信号。
plot(transformed, lw=2)
plot(shifted, lw=3)
show()    #使用Matplotlib分别绘制变换和移频处理后的信号。

# numpy.fft.fftshift(x, axes=None)[source]
# Shift the zero-frequency component to the center of the spectrum.

# This function swaps half-spaces for all axes listed (defaults to all). Note that y[0] is the Nyquist component only if len(x) is even.

# Parameters:
# x : array_like
# Input array.

# axes : int or shape tuple, optional
# Axes over which to shift. Default is None, which shifts all axes.

# Returns:
# y : ndarray
# The shifted array.

# See also
# ifftshift
# The inverse of fftshift.
# Examples

# >>>
# >>> freqs = np.fft.fftfreq(10, 0.1)
# >>> freqs
# array([ 0.,  1.,  2.,  3.,  4., -5., -4., -3., -2., -1.])
# >>> np.fft.fftshift(freqs)
# array([-5., -4., -3., -2., -1.,  0.,  1.,  2.,  3.,  4.])
# Shift the zero-frequency component only along the second axis:

# >>>
# >>> freqs = np.fft.fftfreq(9, d=1./9).reshape(3, 3)
# >>> freqs
# array([[ 0.,  1.,  2.],
       # [ 3.,  4., -4.],
       # [-3., -2., -1.]])
# >>> np.fft.fftshift(freqs, axes=(1,))
# array([[ 2.,  0.,  1.],
       # [-4.,  3.,  4.],
       # [-1., -3., -2.]])