import os
import json
import rospy
import rosparam
from random import randint
from time import time, sleep
from std_msgs.msg import Int8
from copy import deepcopy
from collections import namedtuple
from math import radians, sin, cos, atan
from fields_objects.Robot import RosserialClient
from mars.msg import RobotMovement, ConnectionCfg,\
    ActionAtTime, RobotKyePoints, RobotId, FineTuneConnection, Connection as ConnectionMsg
from geometry_utils.geometry import *
from vision.SmoothFilter import Smoother, RobotSmoother, PointSmoother
from fields_objects.Path import RobotMovementHandler, DstPositionHandler
from fields_objects.Path import OptimalPathFinder
from vision.visualization import visualize

class FormationStateMachine(object):
    def __init__(self, robots_num, config_path, states=True, disconnect_mod=True, style="trimod"):
        self.formation_handler = FormationHandler()
        self.formation_handler.load_formation_config_file(config_path)
        self.robots_num = robots_num
        self.path_planner = OptimalPathFinder()
        self.robots = None
        self.node_id = 0
        self.state = "robots_control"
        self.connection_type = "single_connection"
        self.msg_to_show = None # visualize
        self.states_mode = states
        self.node_refreshers_dict = {}

        conn_cfg_topic = "connectCfg/"
        robot_movement_topic = "robot_movement/"
        conn_topic = "connection/"
        fine_tune_topic = "fine_tune_connection/"
        reconfiguration_topic = "reconfiguration/"
        grub_connectors_topic = "grub/"
        self.connection_cfg_publishers = self.create_publishers_dict(conn_cfg_topic, ConnectionCfg)
        self.robot_movement_publishers = self.create_publishers_dict(robot_movement_topic, ActionAtTime)
        self.connection_publishers = self.create_publishers_dict(conn_topic, ConnectionMsg)
        self.fine_tune_publishers = self.create_publishers_dict(fine_tune_topic, FineTuneConnection)
        self.reconfiguration_publishers = self.create_publishers_dict(reconfiguration_topic, Int8)
        self.grub_connectors_publishers = self.create_publishers_dict(grub_connectors_topic, Int8)

        self.actions_handler = None

        self.double_conn_params2 = None

        self.disconnect_mod = disconnect_mod
        self.configuring_robots = {}
        self.fine_tune_connecting_robots = {}
        self.connected_robots = {}
        self.connected_to_ros = {}
        self.disconnected = True if self.disconnect_mod else False #TODO
        self.on_pos_pub = rospy.Publisher("on_position", Int8, queue_size=1)

        self.reconfigure_angle = rosparam.get_param("reconfigure_angle")

        self.style = style

        self.was_delay = False
        self.stoped = False
        self.no_man = rosparam.get_param("no_man")

    def robots_control_callback(self, msg):
        robots = msg.robots
        self.update_robots_data(robots)
        self.update_nodes_dict()
        if self.was_delay and not self.stoped:
            if not self.disconnected:
                if self.state == "robots_control":
                    self.active_node_callback()
                    self.robots_control()
                elif self.state == "configure_connectors":
                    self.set_connectors_configuration()
                elif self.state == "on_position":
                    self.on_pos_pub.publish(self.formation_handler.active_node.robot.id)
                elif self.state == "do_connect":
                    self.pub_do_connect_msg()
                elif self.state == "formation_action":
                    self.do_formation_action()
                elif self.state == "reconfiguration":
                    self.do_reconfigure()
                elif self.state == "finish":
                    ch_mod = rosparam.get_param("change_mode")
                    if ch_mod:
                        self.change_mode()
                        print("CHANGE_MODE") # TODO
                    else:
                        print("FINISH")
            # elif self.state in ["single_connection", "double_connection_state_1", "double_connection_state_2"]:
            #     self.fine_tune_connection()
            # elif self.state == "connection":
            #     id1, id2 = self.formation_handler.publish_connection_msg(self.formation_handler.active_node.id,
            #                                                              self.connection_publishers)
            #     self.connected_robots[id1[0]] = False
            #     self.connected_robots[id2[0]] = False
        connected = [client.id for client in self.connected_to_ros.values() if not client.is_disconnect()]
        disconnected = [client.id for client in self.connected_to_ros.values() if client.is_disconnect()]
        visualize(self.msg_to_show, self.formation_handler, self.robots,
                  self.path_planner, self.state, self.robots_num, connected, disconnected)

    def change_mode(self):
        if self.style == "snake":
            center_r_id = self.formation_handler.nodes_dict[0].robot.id
            left_r_id = self.formation_handler.nodes_dict[1].robot.id
            right_r_id = self.formation_handler.nodes_dict[2].robot.id
            # self.reboot_reconfig(left_r_id, right_r_id)
            # self.timer(10)
            self.reboot_cfg(center_r_id)
            self.timer(7)
            self.reboot_position(left_r_id, right_r_id)
            self.timer(7)
            self.reboot_nodes_dict()
            self.reboot_style()
            self.reboot_node_id()
            self.set_state("robots_control")

    def all_nodes_connected(self):
        if len(self.formation_handler.nodes_dict.keys()) == self.robots_num:
            return True
        else:
            return False

    def stop_all(self, msg):
        self.stoped = msg.data

    def do_formation_action(self):
        self.actions_handler.do_actions()
        self.set_state("reconfiguration")

    def do_reconfigure(self):
        # left_node = self.formation_handler.nodes_dict.get(1)
        # right_node = self.formation_handler.nodes_dict.get(2)
        #
        # left_robot = left_node.robot if left_node is not None else None
        # right_robot = right_node.robot if right_node is not None else None
        #
        # left_robot_publisher = self.reconfiguration_publishers[left_robot.id] if left_robot else None
        # right_robot_publisher = self.reconfiguration_publishers[right_robot.id] if right_robot else None
        #
        # left_msg, right_msg = Int8(), Int8()
        # if left_robot_publisher:
        #     left_msg.data = self.reconfigure_angle
        #     left_robot_publisher.publish(left_msg)
        # if right_robot_publisher:
        #     right_msg.data = self.reconfigure_angle
        #     right_robot_publisher.publish(right_msg)
        # self.timer(9)
        # if left_robot_publisher:
        #     left_msg.data = -self.reconfigure_angle
        #     left_robot_publisher.publish(left_msg)
        # if right_robot_publisher:
        #     right_msg.data = -self.reconfigure_angle
        #     right_robot_publisher.publish(right_msg)
        # # for i in range(3):
        # #     left_robot_publisher.publish(left_msg)
        # #     right_robot_publisher.publish(right_msg)
        self.set_state("finish")

    def pub_do_connect_msg(self):
        node_id = self.formation_handler.active_node.id
        if node_id != 0:
            connection_type, params = self.formation_handler.set_connection_params(node_id)

            # connection_type, params = self.set_connection_params(node_id)
            self.formation_handler.publish_single_connection_msg(params, self.connection_publishers)
        # publish_single_connection_msg

    def reconfigured_callback(self, msg):
        # data = msg.data if not isinstance(msg, int) else msg
        # robot_id = self.formation_handler.nodes_dict[0].robot.id
        # if data == robot_id:
        #     self.init_actions_handler()
        #     self.set_state("formation_action")
        print("reconfigured_callback???? 130")

    def init_actions_handler(self):
        left_node = self.formation_handler.nodes_dict.get(1)
        right_node = self.formation_handler.nodes_dict.get(2)

        left_robot = left_node.robot if left_node is not None else None
        right_robot = right_node.robot if right_node is not None else None

        center_robot = self.formation_handler.nodes_dict[0].robot
        self.actions_handler = TriModActionsHandler(left_robot, right_robot, center_robot, self.style)
        self.actions_handler.init_actions_publishers(self.robot_movement_publishers)

    def ready_to_connect_callback(self, msg):
        # print("Ready_to_connect", msg, self.formation_handler.active_node.robot.id)
        data = msg.data if not isinstance(msg, int) else msg
        if data == self.formation_handler.active_node.robot.id:
            self.set_state("do_connect")

    def connect_to_ros_callback(self, msg):
        # if not isinstance(msg, int):
        #     print("4, old_format!")
        data = msg.data if not isinstance(msg, int) else msg
        print(data)
        if data not in self.connected_to_ros.keys():
            self.connected_to_ros[data] = RosserialClient(data)
        else:
            self.connected_to_ros[data].update_time()
        disconnected = [client for client in self.connected_to_ros.values() if client.is_disconnect()]
        # self.disconnected = False
        if len(self.connected_to_ros) == self.robots_num and len(disconnected) == 0:
            self.disconnected = False
            if self.no_man and not self.was_delay:
                print("TIMER")
                self.timer(rosparam.get_param("timer"), True)
            self.was_delay = True
        else:
            self.disconnected = True if self.disconnect_mod else False # TODO

    def set_state(self, state):
        if self.states_mode:
            self.state = state

    def get_conn_side_to_grub_connectors(self):
        if self.formation_handler.active_node is not None:
            if self.formation_handler.active_node.id == 1:
                return 1
            elif self.formation_handler.active_node.id == 2:
                return 2

    def timer(self, timer_time, pr=False):
        if not pr:
            start_time = time()
            while time() - start_time < timer_time:
                pass
        else:
            for i in range(timer_time):
                print(timer_time - i)
                sleep(1)

    def connected_callback(self, msg):
        print(randint(0,5), 'Connected', msg)
        # if not isinstance(msg, int):
        #     print("2, old_format!")
        data = msg.data if not isinstance(msg, int) else msg

        self.connected_robots[data] = True
        # if all(self.connected_robots.values()):
        if self.formation_handler.active_node is not None and self.state != "formation_action" and  \
                                                              self.state != "reconfiguration":
            if data == self.formation_handler.active_node.robot.id:
                # if not self.all_nodes_connected():
                self.connected_robots = {}
                grub_publisher = self.grub_connectors_publishers[self.formation_handler.nodes_dict[0].robot.id]
                grub_conn_side = self.get_conn_side_to_grub_connectors()
                grub_publisher.publish(grub_conn_side)
                print("GRUB:\n\trobot: {}\n\tgrub_side: {}".format(self.formation_handler.nodes_dict[0].robot.id,
                                                                   grub_conn_side))
                self.timer(5)
                # if self.state == "single_connection" or self.state == "double_connection_state_2":
                self.formation_handler.del_active_node()
                if len(self.formation_handler.nodes_dict.keys()) == self.robots_num:
                    self.init_actions_handler()
                    self.set_state("formation_action")
                else:
                    self.set_state("robots_control")
                # else:
                #     self.set_state("double_connection_state_2")
            # else:
            #     self.init_actions_handler()
            #     self.set_state("formation_action")

    def connectors_configure_callback(self, msg):
        print("Configured", msg)
        # if not isinstance(msg, int):
        #     print("3, old_format!")
        data = msg.data if not isinstance(msg, int) else msg

        if self.state == "configure_connectors":
            self.configuring_robots[data] = True
            if all(self.configuring_robots.values()):
                self.connected_robots = {k: False for k in self.configuring_robots.keys()}
                self.configuring_robots = {}
                # self.set_state(self.connection_type)
                self.set_state("on_position")

    def fine_tune_connection_callback(self, msg):
        # if not isinstance(msg, int):
        #     print("1, old_format!")
        data = msg.data if not isinstance(msg, int) else msg

        self.fine_tune_connecting_robots[data] = True
        if self.state == "single_connection":
            if all(self.fine_tune_connecting_robots.values()):
                self.fine_tune_connecting_robots = {}
                self.set_state("connection")
        else:
            pass
            # if self.state == "double_connection_state_1":
            #     if all(self.fine_tune_connecting_robots.values()):
            #         id1, id2 = self.formation_handler.publish_single_connection_msg(self.double_conn_params2,
            #                                                                         self.connection_publishers)
            #         self.fine_tune_connecting_robots = {}
            #         self.fine_tune_connecting_robots[id1] = False
            #         self.fine_tune_connecting_robots[id2] = False
            #         self.set_state("double_connection_state_2")
            # else:
            #     if all(self.fine_tune_connecting_robots.values()):
            #         self.fine_tune_connecting_robots = {}
            #         self.set_state("robots_control")

    def active_node_callback(self):
        if len(self.formation_handler.nodes_dict.keys()) != self.robots_num:
            if not self.disconnected:
                if not self.is_active_node():
                    self.create_active_node()
                else:
                    node_id = self.formation_handler.active_node.id
                    if node_id in self.node_refreshers_dict.keys():
                        counts = self.node_refreshers_dict[node_id]
                        refreshed = self.formation_handler.refresh_node_heading_and_position(node_id, counts)
                    else:
                        refreshed = self.formation_handler.refresh_node_heading_and_position(node_id, 0)
                    if refreshed:
                        if node_id not in self.node_refreshers_dict.keys():
                            self.node_refreshers_dict[node_id] = 1
                        else:
                            self.node_refreshers_dict[node_id] += 1
                        if self.is_active_robot():
                            self.stop_robot()
                            self.formation_handler.active_node.robot = None
        else:
            self.init_actions_handler()
            self.set_state("formation_action")

    def robots_control(self):
        if len(self.formation_handler.nodes_dict.keys()) != self.robots_num:
            if self.is_active_node() and self.is_active_robot():
                self.active_robot_control()
            else:
                if self.formation_handler.active_node.id == 0:
                    dst_position = self.formation_handler.active_node.dst_position
                else:
                    dst_position = self.formation_handler.create_pre_finish_point()
                self.non_active_robot_control(dst_position)
        else:
            self.init_actions_handler()
            self.set_state("formation_action")
            # self.set_state("reconfiguration")

    def active_robot_control(self):
        if self.robot_on_position_and_on_heading():
            self.stop_robot()
            node_id = self.formation_handler.active_node.id
            if node_id != 0:
                if not self.states_mode:
                    self.formation_handler.del_active_node()
                self.set_state("configure_connectors")
            else:
                self.formation_handler.del_active_node()
        else:
            self.move_robot()

    def non_active_robot_control(self, dst_position):
        if not self.find_and_set_robot(dst_position):
            self.unstack_robots(dst_position)
            # pass

    def unstack_robots(self, dst_position):
        used_robots_ids = [node.robot.id for node in self.formation_handler.nodes_dict.values()]
        not_used_robots = [robot for robot in self.robots if robot.id not in used_robots_ids]
        self.path_planner.set_robots(not_used_robots)
        robot_id, path_point = self.path_planner.get_move_params_for_stacked_robots(dst_position)
        self.move_robot(robot_id, path_point)

    def find_and_set_robot(self, dst_position):
        self.init_planner()  # we're need this init to work with obstacles
        used_robots_ids = [node.robot.id for node in self.formation_handler.nodes_dict.values()]
        not_used = [robot for robot in self.robots if robot.id not in used_robots_ids]
        if self.formation_handler.active_node.id == 0:
            pre_dst_position_robot_id = self.check_position_in_obstacle(dst_position, not_used)
            if pre_dst_position_robot_id is not None:
                path, robot_id = [dst_position], pre_dst_position_robot_id
            else:
                path, robot_id = self.find_shorted_path_and_robot_id(dst_position)
            if robot_id is not None and path:
                self.set_robot_and_path(robot_id, path)
                return True
            else:
                return False
        else:
            dst_position_robot_id = self.check_position_in_obstacle(self.formation_handler.active_node.dst_position, not_used)
            pre_dst_position_robot_id = self.check_position_in_obstacle(dst_position, not_used)
            if dst_position_robot_id is not None:
                path, robot_id = [self.formation_handler.active_node.dst_position], dst_position_robot_id
            elif pre_dst_position_robot_id is not None:
                path, robot_id = [dst_position], pre_dst_position_robot_id
            else:
                path, robot_id = self.find_shorted_path_and_robot_id(dst_position)
            if robot_id is not None and path:
                self.set_robot_and_path(robot_id, path)
                return True
            else:
                return False

    def set_robot_and_path(self, robot_id, path):
        self.set_active_robot(robot_id)
        self.set_path(path)

    def check_position_in_obstacle(self, position, robots):
        nearest_robot = self.path_planner.get_nearest_robot_with_finish_inside_obs(position, robots)
        return nearest_robot

    def update_robots_data(self, robots):
        self.robots = robots

    def update_nodes_dict(self):
        self.formation_handler.update_nodes_dict(self.robots)

    def is_active_node(self):
        if self.formation_handler.active_node is not None:
            return True
        else:
            return False

    def is_active_robot(self):
        if self.formation_handler.active_node.robot is not None:
            return True
        else:
            return False

    def create_active_node(self):
        ret_val = self.formation_handler.refresh_active_node(self.node_id)
        self.node_id = self.node_id + 1 if ret_val else self.node_id

    def refresh_node_heading_and_position(self):
        node_id = self.formation_handler.active_node.id
        refreshed = self.formation_handler.refresh_node_heading_and_position(node_id)
        return refreshed

    def create_pre_dst_position(self):
        return self.formation_handler.create_pre_finish_point()

    def init_planner(self):
        self.path_planner.init_planner()

    def find_shorted_path_and_robot_id(self, pre_dst_position):
        used_robots_ids = [node.robot.id for node in self.formation_handler.nodes_dict.values()]
        self.path_planner.set_finish_position(pre_dst_position)
        self.path_planner.set_robots(self.robots)
        self.path_planner.set_used_robots(used_robots_ids)
        actual_robot_id, shortest_path = self.path_planner.create_shortest_path()
        return shortest_path, actual_robot_id

    def set_active_robot(self, robot_id):
        robot = [robot for robot in self.robots if robot.id == robot_id][0]
        self.formation_handler.set_active_robot(robot)

    def set_path(self, path):
        self.formation_handler.set_path(path)

    def set_connectors_configuration(self):
        if self.formation_handler.active_node.id != 0:
            cfg_robots_ids = self.formation_handler.pub_connection_cfg(self.connection_cfg_publishers)
            for i in cfg_robots_ids:
                if not i in self.configuring_robots.keys():
                    self.configuring_robots[i] = False

    def robot_on_position_and_on_heading(self):
        return self.formation_handler.node_is_ready()

    def move_robot(self, robot_id=None, dst_position=None):
        if robot_id is None and dst_position is None:
            for robot in self.robots:
                if robot.id == self.formation_handler.active_node.robot.id:
                    self.formation_handler.update_active_node(robot)
                    msg = self.formation_handler.active_node.prepare_msg()
                    self.msg_to_show = (robot.id, msg)
                    self.formation_handler.publish_msg(msg, self.robot_movement_publishers)
        else:
            msg = self.prepare_msg_for_non_active_robot(robot_id, dst_position)
            self.msg_to_show = (robot_id, msg)
            self.formation_handler.publish_msg(msg, self.robot_movement_publishers, robot_id)

    def prepare_msg_for_non_active_robot(self, robot_id, dst_position):
        robot = [robot for robot in self.robots if robot.id == robot_id][0]
        angle = degrees(get_angle_by_3_points(robot.direction, dst_position, robot.center))
        sign = get_angle_sign_pt_to_line(dst_position, robot.direction, robot.center)
        angle *= sign
        msg = ActionAtTime()
        msg.angle = self.angle_to_sector(angle_to_180(angle))
        msg.movement = True
        msg.rotation = False
        msg.time = self.get_moving_time(get_distance_between_points(robot.center, dst_position))
        return msg

    def stop_robot(self):
        msg = self.formation_handler.active_node.prepare_stop_msg()
        self.msg_to_show = (self.formation_handler.active_node.robot.id, msg)
        self.formation_handler.publish_msg(msg, self.robot_movement_publishers)

    def fine_tune_connection(self):
        if self.state in ["single_connection", "double_connection_state_2", "double_connection_state_1"]:
            node_id = self.formation_handler.active_node.id
            if node_id != 0:
                self.on_pos_pub.publish(node_id)
                # conn_type, params = self.formation_handler.set_connection_params(node_id)
                # if conn_type == "single_connection":
                #     # conn_to              conn_from
                #     [id1, conn_side1], [id2, conn_side2] = self.formation_handler.publish_single_connection_msg(params, self.connection_publishers)
                #     fine_tune_msg = self.prepare_full_fine_tune_msg(id1, conn_side1, id2, conn_side2)
                #     self.fine_tune_publisher[id2].publish(fine_tune_msg)
                #     if id1 not in self.fine_tune_connecting_robots.keys():
                #     # print(id1, id2)
                #         self.fine_tune_connecting_robots[id1] = False
                #     if id2 not in self.fine_tune_connecting_robots.keys():
                #         self.fine_tune_connecting_robots[id2] = False
                #     # self.connected_robots[id1[0]] = False
                #     # self.connected_robots[id2[0]] = False
                #     # self.set_state("single_connection")
                # else:
                #     pass
                    # if self.state != "double_connection_state_2":
                    #     params1 = params.node1_params
                    #     # conn_to              conn_from
                    #     [id1, conn_side1], [id2, conn_side2] = self.formation_handler.publish_single_connection_msg(params1,
                    #                                                                     self.connection_publishers)
                    #     fine_tune_msg = self.prepare_full_fine_tune_msg(id1, conn_side1, id2, conn_side2)
                    #     self.fine_tune_publisher[id2].publish(fine_tune_msg)
                    #     # if id1 not in self.connecting_robots.keys():
                    #     #     self.connecting_robots[id1] = False
                    #     if id2 not in self.fine_tune_connecting_robots.keys():
                    #         self.fine_tune_connecting_robots[id2] = False
                    #     if self.state != "double_connection_state_1":
                    #         self.set_state("double_connection_state_1")
                    # else:
                    #     # conn_to              conn_from
                    #     params2 = params.node2_params
                    #     [id1, conn_side1], [id2, conn_side2] = self.formation_handler.publish_single_connection_msg(params2,
                    #                                                                     self.connection_publishers)
                    #     fine_tune_msg = self.prepare_full_fine_tune_msg(id1, conn_side1, id2, conn_side2)
                    #     self.fine_tune_publisher[id2].publish(fine_tune_msg)
                    #     # if id1 not in self.connecting_robots.keys():
                    #     #     self.connecting_robots[id1] = False
                    #     if id2 not in self.fine_tune_connecting_robots.keys():
                    #         self.fine_tune_connecting_robots[id2] = False

    def prepare_full_fine_tune_msg(self, to_id, to_side, from_id, from_side):
        conn_to_k_points = self.prepare_robot_k_points_msg(to_id, to_side)
        conn_from_k_points = self.prepare_robot_k_points_msg(from_id, from_side)
        return self.prepare_fine_tune_conn_msg(conn_to_k_points, conn_from_k_points)

    def prepare_robot_k_points_msg(self, robot_id, side):
        robot = [r for r in self.robots if r.id == robot_id][0]
        msg = RobotKyePoints()
        msg.center = robot.center
        msg.side_point = self.get_robot_side_position(robot, side)
        return msg

    def prepare_fine_tune_conn_msg(self, conn_to_k_points, conn_from_k_points):
        msg = FineTuneConnection()
        msg.conn_to = conn_to_k_points
        msg.conn_from = conn_from_k_points
        return msg

    def create_publishers_dict(self, topic, msg_type, robots_num=10):
        return {i: self.create_publisher(topic, i, msg_type) for i in range(robots_num)}

    @staticmethod
    def create_publisher(topic, robot_id, mst_type):
        return rospy.Publisher(topic + str(robot_id), mst_type, queue_size=1)

    @staticmethod
    def angle_to_sector(angle):
        secs = [-180, -150, -120, -60, -30, 30, 60, 120, 150, 181]
        low_val, high_val = None, None
        for i in range(1, len(secs)):
            if angle >= secs[i - 1] and angle < secs[i]:
                low_val, high_val = secs[i - 1], secs[i]
                break
        return (low_val + high_val) // 2

    @staticmethod
    def get_moving_time(distance):
        moving_on_240_pix = rosparam.get_param("moving_on_240_pix")
        msg_time_scale = rosparam.get_param("msg_time_scale")
        time = (distance / 240.0) * moving_on_240_pix * 1000 * msg_time_scale
        return round(time, 2)

    @staticmethod
    def get_robot_side_position(robot, side):
        if side == "center":
            return robot.center
        elif side == "front_side":
            return robot.front_side
        elif side == "back_side":
            return robot.back_side
        elif side == "left_side":
            return robot.left_side
        elif side == "right_side":
            return robot.right_side
        else:
            return None

    # rebooting!
    def reboot_nodes_dict(self):
        self.formation_handler.nodes_dict = {}

    def reboot_style(self):
        self.style = "trimod"
        config_path = os.path.join(os.getcwd(), os.pardir, "config", "{}.json".format(self.style))
        self.formation_handler.load_formation_config_file(config_path)

    def reboot_node_id(self):
        self.node_id = 0

    def reboot_position(self, l_r_id, r_r_id):
        l_r_pub = self.robot_movement_publishers[l_r_id]
        r_r_pub = self.robot_movement_publishers[r_r_id]
        l_msg = ActionAtTime()
        l_msg.time = 5000
        l_msg.angle = -90
        l_msg.movement = True
        l_msg.rotation = False

        r_msg = ActionAtTime()
        r_msg.time = 5000
        r_msg.angle = 90
        r_msg.movement = True
        r_msg.rotation = False

        l_r_pub.publish(l_msg)
        r_r_pub.publish(r_msg)

    def reboot_reconfig(self, l_r_id, r_r_id):
        rec_angle = rosparam.get_param("reconfigure_angle")
        l_reconf_pub = self.reconfiguration_publishers[l_r_id]
        r_reconf_pub = self.reconfiguration_publishers[r_r_id]
        l_reconf_pub.publish(-rec_angle)
        r_reconf_pub.publish(-rec_angle)

    def reboot_cfg(self, center_robot_id):
        l_cfg_msg = ConnectionCfg()
        l_cfg_msg.connection_side_id = 3
        l_cfg_msg.connection_state = 0
        r_cfg_msg = ConnectionCfg()
        r_cfg_msg.connection_side_id = 1
        r_cfg_msg.connection_state = 0
        cfg_pub = self.connection_cfg_publishers[center_robot_id]
        cfg_pub.publish(l_cfg_msg)
        self.timer(7)
        cfg_pub.publish(r_cfg_msg)

SingleConnParams = namedtuple('SingleConnParams', ["conn_to_node_id",
                                                   "conn_to_node_side",
                                                   "conn_to_node_side_point",
                                                   "conn_to_node_position",
                                                   "conn_to_node_heading",
                                                   "conn_from_node_side"])

DoubleConnParams = namedtuple("DoubleConnParams", ["node1_params",
                                                   "node2_params"])

class FormationNode(object):
    def __init__(self, id, dst_position, dst_heading, pre_dst_position, connection_type="single_connection"):
        self.id = id
        self.connection_type = connection_type
        self.connection_params = None

        self.dst_position_smoother = PointSmoother(N=8)
        self.dst_heading_smoother = Smoother(N=8)
        self.robot_smoother = RobotSmoother(N=8)

        self.dst_position = self.dst_position_smoother.get_smoothed_point(dst_position)
        self.dst_heading = self.dst_heading_smoother.get_smoothed_value(dst_heading)
        self.pre_dst_position = pre_dst_position

        self.distance_eps = rosparam.get_param("distance_eps")
        self.angle_eps = rosparam.get_param("angle_eps")
        self.dist_between_markers_pair_centers = rosparam.get_param("dist_between_markers_pair_centers")

        self.rotation_on_360_time = rosparam.get_param("rotation_on_360")
        self.moving_on_240_pix = rosparam.get_param("moving_on_240_pix")
        self.msg_time_scale = rosparam.get_param("msg_time_scale")

        self.robot = None
        self.path = None
        self.dst_position_handler = DstPositionHandler()
        self.path_passing_handler = RobotMovementHandler()

    def smooth_dst_position(self, position):
        smoothed_point = self.dst_position_smoother.get_smoothed_point(position)
        return smoothed_point

    def smooth_dst_heading(self, heading):
        smoothed_heading = self.dst_heading_smoother.get_smoothed_value(heading)
        return smoothed_heading

    def set_robot(self, robot):
        self.robot = robot

    def set_path(self, path):
        self.path = path

    def init_path_passing_handler(self):
        self.path_passing_handler.init_handler(self.robot, self.path) #, self.distance_eps, self.angle_eps)

    def init_dst_position_moving_handler(self):
        self.dst_position_handler.init_handler(self.robot, self.dst_position, self.dst_heading, self.connection_type)

    def update_node(self, robot):
        self.robot = robot
        # self.robot = self.robot_smoother.smooth_robot(robot)
        if not len(self.path_passing_handler.path):
            self.dst_position_handler.update(self.robot)
        else:
            self.path_passing_handler.update(self.robot)

    def prepare_msg(self):
        if not len(self.path_passing_handler.path):
            if get_distance_between_points(self.robot.center, self.dst_position):
                self.distance_eps = 3
            else:
                self.distance_eps = rosparam.get_param("distance_eps")
            msg = self.dst_position_handler.prepare_msg()
            self.distance_eps = rosparam.get_param("distance_eps")
        else:
            msg = self.path_passing_handler.prepare_msg()
        return msg

    def prepare_stop_msg(self):
        msg = ActionAtTime()
        msg.movement = False
        msg.rotation = False
        msg.angle = 0
        msg.time = 10
        return msg

    def robot_on_heading(self):
        heading_point = self.degrees_to_point(self.dst_heading, self.robot.center)
        angle = degrees(get_angle_by_3_points(self.robot.direction, heading_point, self.robot.center))
        if angle <= self.angle_eps:
            return True
        else:
            return False

    def robot_on_position(self):
        if get_distance_between_points(self.robot.center, self.dst_position) <= self.distance_eps:
            return True
        else:
            return False

    @staticmethod
    def degrees_to_point(angle, point, radius=10):
        rad_angle = radians(angle)
        dy = cos(rad_angle) * radius
        dx = sin(rad_angle) * radius
        return Point(point.x + dx, point.y - dy)

    @staticmethod
    def point_to_angle(center_point, point):
        dx = point.x - center_point.x
        dy = - (point.y - center_point.y)
        if not dy:
            return 90 if dx >= 0 else 270
        else:
            tan_a = float(dx) / float(dy)
        a_tan = atan(tan_a)
        deg_angle = degrees(a_tan)
        if point.y - center_point.y <= 0:
            return deg_angle
        else:
            return 180 + deg_angle

class FormationHandler(object):
    def __init__(self):
        self.active_node = None
        self.active_robot = None
        self.nodes_dict = {}
        self.formation_config = None
        self.connection_pair = []
        self.last_published_move_msg = None
        self.last_task_duration = 0
        self.last_task_pub_time = 0
        self.nodes_smoothers = {}
        self.costil_mode = rosparam.get_param("costil_mode")

    def set_path(self, path):
        self.active_node.set_path(path)
        self.active_node.init_path_passing_handler()
        self.active_node.init_dst_position_moving_handler() # TODO pre_dst_position

    def prepare_connectors_cfg_msg(self, conn_to_node_side, conn_from_side):
        sides_dict = {"front_side":0, "right_side":1, "back_side":2, "left_side":3}
        configs_dict = {"mama":0, "papa":1}

        conn_to_cfg_msg = ConnectionCfg()
        conn_from_cfg_msg = ConnectionCfg()

        if conn_to_node_side in ["front_side", "back_side"]:
            conn_to_cfg_msg.connection_side_id = sides_dict[conn_to_node_side]
            conn_to_cfg_msg.connection_state = configs_dict["papa"]
            conn_from_cfg_msg.connection_side_id = sides_dict[conn_from_side]
            conn_from_cfg_msg.connection_state = configs_dict["mama"]
        elif conn_from_side in ["front_side", "back_side"]:
            conn_from_cfg_msg.connection_side_id = sides_dict[conn_from_side]
            conn_from_cfg_msg.connection_state = configs_dict["papa"]
            conn_to_cfg_msg.connection_side_id = sides_dict[conn_to_node_side]
            conn_to_cfg_msg.connection_state = configs_dict["mama"]
        else:
            conn_to_cfg_msg.connection_side_id = sides_dict[conn_to_node_side]
            conn_to_cfg_msg.connection_state = configs_dict["mama"]
            conn_from_cfg_msg.connection_side_id = sides_dict[conn_from_side]
            conn_from_cfg_msg.connection_state = configs_dict["papa"]

        return conn_to_cfg_msg, conn_from_cfg_msg

    def corners(self, conn_to_robot_id, conn_from_robot_id, conn_to_msg, conn_from_msg, cfg_publishers_dict):
        conn_to_publisher = cfg_publishers_dict[conn_to_robot_id]
        conn_from_publisher = cfg_publishers_dict[conn_from_robot_id]
        conn_to_publisher.publish(conn_to_msg)
        conn_from_publisher.publish(conn_from_msg)

    def create_pre_finish_point(self):
        if self.active_node.id == 0:
            pre_finish_point = self.active_node.dst_position
        else:
            conn_type, params = self.set_connection_params(self.active_node.id)
            if conn_type == "single_connection":
                pre_finish_point, _ = self.create_pre_finish_pt_for_single_connection(params)
                self.active_node.dst_position_handler.set_pre_dst_position(pre_finish_point)
            elif conn_type == "double_connection":
                pre_finish_point, nodes_lst = self.create_pre_finish_pt_for_double_connection(params)
                tmp_dst_pos = self.get_tmp_pos_point_for_double_conn(pre_finish_point, *nodes_lst)
                self.active_node.dst_position_handler.set_pre_dst_position(tmp_dst_pos)
                self.active_node.conn_to_node_center1, self.active_node.conn_to_node_center2 = nodes_lst
            else:
                pre_finish_point = None
        return pre_finish_point

    def create_pre_finish_pt_for_single_connection(self, conn_params):
        conn_to_robot = self.nodes_dict[conn_params.conn_to_node_id].robot
        conn_to_robot_center = conn_to_robot.center
        node_dst_position = self.active_node.dst_position

        dx = node_dst_position.x - conn_to_robot_center.x
        dy = node_dst_position.y - conn_to_robot_center.y

        lr_sides = ["left_side", "right_side"]

        booth_sides = conn_params.conn_to_node_side in lr_sides and conn_params.conn_from_node_side in lr_sides

        scale = 0.8 if booth_sides else 0.7
        x = node_dst_position.x + dx * scale
        y = node_dst_position.y + dy * scale
        return Point(x, y), conn_to_robot_center

    def create_pre_finish_pt_for_double_connection(self, double_conn_params_list):
        params1, params2 = double_conn_params_list
        pt1, node_pos1 = self.create_pre_finish_pt_for_single_connection(params1)
        pt2, node_pos2 = self.create_pre_finish_pt_for_single_connection(params2)
        mean_x, mean_y = np.mean([pt1.x, pt2.x]), np.mean([pt1.y, pt2.y])
        return Point(mean_x, mean_y), [node_pos1, node_pos2]

    def get_tmp_pos_point_for_double_conn(self, dst_pos, node1_pos, node2_pos):
        dx1, dy1 = dst_pos.x - node1_pos.x, dst_pos.y - node1_pos.y
        dx2, dy2 = dst_pos.x - node2_pos.x, dst_pos.y - node2_pos.y
        tmp_x1, tmp_y1 = node1_pos.x - dx1, node1_pos.y - dy1
        tmp_x2, tmp_y2 = node2_pos.x - dx2, node2_pos.y - dy2
        mean_x = np.mean([tmp_x1, tmp_x2])
        mean_y = np.mean([tmp_y1, tmp_y2])
        return Point(mean_x, mean_y)

    def is_any_robot_on_position(self, robots_list, position):
        for robot in robots_list:
            if get_distance_between_points(robot.center, position) <= self.active_node.distance_eps:
                return robot.id
        else:
            return None

    def publish_msg(self, msg, publishers_dict, robot_id=None):
        if robot_id is None:
            publisher = publishers_dict[self.active_node.robot.id]
        else:
            publisher = publishers_dict[robot_id]
        publisher.publish(msg)

    def del_active_node(self):
        if self.active_node is not None and self.active_node.robot is not None:
            self.nodes_dict[self.active_node.id] = deepcopy(self.active_node)
            self.active_node = None
            self.active_robot = None

    def refresh_active_node(self, node_id):
        if node_id == 0:
            node = self.create_base_node()
        else:
            node = self.create_non_base_node(node_id)
        if node:
            self.active_node = node
            return True
        else:
            return False

    def node_is_ready(self):
        if self.active_node.robot_on_heading() and self.active_node.robot_on_position():
            return True
        else:
            return False

    def set_active_robot(self, robot):
        self.active_node.set_robot(robot)
        self.active_robot = self.active_node.robot

    def pub_connection_cfg(self, cfg_publishers_dict):
        if self.active_node.id != 0:
            connection_type, params = self.set_connection_params(self.active_node.id)
            if connection_type == "single_connection":
                conn_from_robot_id, conn_to_robot_id = self.pub_single_conn_cfg_msg(params, cfg_publishers_dict)
                return [conn_from_robot_id, conn_to_robot_id]
            else:
                params_list1, params_list2 = params.node1_params, params.node2_params
                conn_from_robot_id1, conn_to_robot_id1 = self.pub_single_conn_cfg_msg(params_list1, cfg_publishers_dict)
                conn_from_robot_id2, conn_to_robot_id2 = self.pub_single_conn_cfg_msg(params_list2, cfg_publishers_dict)
                return list({conn_from_robot_id1, conn_to_robot_id1, conn_from_robot_id2, conn_to_robot_id2})

    # def pub_do_connect_msg(self, conn_publishers):
    #     if self.active_node.id != 0:
    #         connection_type, params = self.set_connection_params(self.active_node.id)
    #         conn_from_robot_id, conn_to_robot_id = self.pub_single_conn_cfg_msg(params, conn_publishers)

    def pub_single_conn_cfg_msg(self, params, cfg_publishers_dict):
        conn_from_robot_id = self.active_robot.id
        conn_to_robot_id = self.nodes_dict[params.conn_to_node_id].robot.id
        conn_to_cfg_msg, conn_from_cfg_msg = self.prepare_connectors_cfg_msg(params.conn_to_node_side,
                                                                             params.conn_from_node_side)
        self.publish_cfg_msgs(conn_to_robot_id, conn_from_robot_id,
                              conn_to_cfg_msg, conn_from_cfg_msg, cfg_publishers_dict)
        return conn_from_robot_id, conn_to_robot_id

    def publish_cfg_msgs(self, conn_to_robot_id, conn_from_robot_id, conn_to_msg, conn_from_msg, cfg_publishers_dict):
        conn_to_publisher = cfg_publishers_dict[conn_to_robot_id]
        conn_from_publisher = cfg_publishers_dict[conn_from_robot_id]

        conn_to_publisher.publish(conn_to_msg)
        conn_from_publisher.publish(conn_from_msg)

    def publish_connection_msg(self, node_id, conn_publishers_dict):
        if node_id != 0:
            connection_type, params = self.set_connection_params(node_id)
            if connection_type == "single_connection":
                id1, id2 = self.publish_single_connection_msg(params, conn_publishers_dict)
                return [id1, id2]
    #
    def publish_single_connection_msg(self, params, publishers_dict):
        sides_dict = {"front_side": 0, "right_side": 1, "back_side": 2, "left_side": 3}

        conn_to_msg, conn_from_msg = ConnectionMsg(), ConnectionMsg()

        conn_to_msg.connection_side = sides_dict[params.conn_to_node_side]
        conn_from_msg.connection_side = sides_dict[params.conn_from_node_side]
        conn_from_robot_id = self.active_robot.id
        conn_to_robot_id = self.nodes_dict[params.conn_to_node_id].robot.id

        conn_to_publisher = publishers_dict[conn_to_robot_id]
        conn_from_publisher = publishers_dict[conn_from_robot_id]
        print("Conn_to", conn_to_msg)
        print("Conn_from", conn_from_msg)
        conn_to_publisher.publish(conn_to_msg)
        conn_from_publisher.publish(conn_from_msg)
        return [conn_to_robot_id, params.conn_to_node_side], [conn_from_robot_id, params.conn_from_node_side]

    def update_active_node(self, robot):
        self.active_robot = robot
        self.active_node.update_node(robot)

    def set_connection_params(self, node_id):
        if str(node_id) in self.formation_config["formation"]["nodes"].keys():
            conn_params = self.formation_config["formation"]["nodes"][str(node_id)]
            double_connection_params = conn_params.get("double_connection")

            if double_connection_params is None:
                return "single_connection", self.get_single_connection_params(conn_params)
            else:
                return "double_connection", self.get_double_connection_params(double_connection_params)
        else:
            return None, None

    def get_single_connection_params(self, connection_params):
        conn_to_node_id, conn_to_node_side = self.parse_connection_to_parametr(connection_params["connection_to"])
        conn_to_node_side_pt = self.get_robot_side_position(self.nodes_dict[conn_to_node_id].robot, conn_to_node_side)
        conn_to_node_position = self.get_robot_side_position(self.nodes_dict[conn_to_node_id].robot, "center")
        conn_from_side = connection_params["connection_from"]
        conn_to_node_heading = self.get_node_heading(conn_to_node_id)
        params = SingleConnParams(conn_to_node_id, conn_to_node_side, conn_to_node_side_pt,
                                  conn_to_node_position, conn_to_node_heading, conn_from_side)
        return params

    def get_double_connection_params(self, double_connection_params):
        connection_params1 = double_connection_params["1"]
        connection_params2 = double_connection_params["2"]
        params_list1 = self.get_single_connection_params(connection_params1)
        params_list2 = self.get_single_connection_params(connection_params2)
        return DoubleConnParams(params_list1, params_list2)

    def refresh_node_heading_and_position(self, node_id, counts):
        if node_id:
            connection_type, params = self.set_connection_params(node_id)
            if connection_type == "single_connection":
                tmp_node = self.create_single_connection_node(node_id, params)
            else:
                tmp_node = self.create_double_connection_node(node_id, params)

            new_position = tmp_node.dst_position
            new_heading = tmp_node.dst_heading

            smoothed_new_position = self.active_node.smooth_dst_position(new_position)
            smoothed_new_heading = self.active_node.smooth_dst_heading(new_heading)

            d_dist = get_distance_between_points(self.active_node.dst_position, smoothed_new_position)
            d_head = abs(self.active_node.dst_heading - smoothed_new_heading)

            if counts <= 30:
                refreshed = True if d_dist >= 15 or d_head >= 10 else False  # TODO: avoid constants
            else:
                refreshed = True if d_dist >= 25 and d_head >= 15 else False
            if refreshed:
                self.active_node.dst_position = smoothed_new_position
                self.active_node.dst_heading = smoothed_new_heading
            else:
                refreshed = False
        else:
            refreshed = False
        return refreshed

    def load_formation_config_file(self, path_to_config_file):
        with open(path_to_config_file) as config_file:
            self.formation_config = json.load(config_file)

    def create_node(self):
        if not len(self.nodes_dict):
            node_id = 0
        else:
            node_id = max(self.nodes_dict.keys()) + 1
        if not node_id:
            new_node = self.create_base_node()
        else:
            new_node = self.create_non_base_node(node_id)
        return new_node

    def set_active_node(self, node):
        self.active_node = node

    def get_nearest_robot(self, robots, position=None):
        used_robots_ids = [node.robot.id for node in self.nodes_dict.values()]
        dst_position = self.active_node.dst_position if not position else position
        nearest_robot = None
        min_dist = 10000
        for robot in robots:
            if robot.id not in used_robots_ids:
                distance = get_distance_between_points(robot.center, dst_position)
                if distance <= min_dist:
                    min_dist = distance
                    nearest_robot = robot
        return nearest_robot

    def update_nodes_dict(self, robots):
        for k, node in self.nodes_dict.items():
            for robot in robots:
                if node.robot is not None:
                    # if node.robot.id is not None:
                    if robot.id == node.robot.id:
                        if not k in self.nodes_smoothers.keys():
                            self.nodes_smoothers[k] = RobotSmoother(15)
                        smoother = self.nodes_smoothers[k]
                        node.robot = smoother.smooth_robot(robot)
                    # node.robot = robot

    def create_base_node(self):
        node_x = self.formation_config["formation"]["nodes"][str(0)]["position"]["x"]
        node_y = self.formation_config["formation"]["nodes"][str(0)]["position"]["y"]
        node_position = Point(node_x, node_y)
        node_heading = self.formation_config["formation"]["nodes"][str(0)]["heading"]
        return FormationNode(0, node_position, node_heading, Point(10, 10), connection_type="base")

    def create_non_base_node(self, node_id):
        connection_type, params = self.set_connection_params(node_id)
        if connection_type == "single_connection":
            return self.create_single_connection_node(node_id, params)
        elif connection_type == "double_connection":
            return self.create_double_connection_node(node_id, params)
        else:
            return None

    def create_single_connection_node(self, node_id, params):
        conn_to_node_id, conn_to_node_side, conn_to_node_side_point, \
        conn_to_node_position, conn_to_node_heading, conn_from_node_side = params

        new_node_position = self.create_new_position(conn_to_node_side, conn_to_node_side_point, conn_to_node_position,
                                                     conn_from_node_side, conn_to_node_id)

        costil_value = rosparam.get_param("costil_value")
        if self.costil_mode:
            # if node_id == 1:
                # new_node_position.y -= int(costil_value*0.3)
            if node_id == 2:
                # new_node_position.y -= int(costil_value // 1.2) #int(costil_value*1.5) #int(costil_value//1.5)
                new_node_position.x += 5
        new_node_heading = self.create_new_heading(conn_to_node_heading, conn_to_node_side, conn_from_node_side)
        return FormationNode(node_id, new_node_position, new_node_heading, conn_to_node_position)

    def create_double_connection_node(self, node_id, double_conn_params_listparams):
        params1, params2 = double_conn_params_listparams

        node1 = self.create_single_connection_node(node_id, params1)
        node2 = self.create_single_connection_node(node_id, params2)

        position1, position2 = node1.dst_position, node2.dst_position
        dx = position1.x - position2.x
        dy = position1.y - position2.y

        pt_x = position1.x - dx
        pt_y = position2.y + dy
        mean_position = Point(pt_x, pt_y)

        heading1, heading2 = node1.dst_heading, node2.dst_heading
        mean_heading = np.mean([heading1, heading2])
        return FormationNode(node_id, mean_position, mean_heading, Point(100, 100), connection_type="double_connection") # TODO double pre_dst

    def parse_connection_to_parametr(self, conn_to):
        conn_to_node_id = conn_to.split("_")[0]
        conn_to_node_side = conn_to[len(conn_to_node_id)+1:]
        return int(conn_to_node_id), conn_to_node_side

    def get_node_heading(self, node_id):
        node = self.nodes_dict[node_id]
        heading_point = node.robot.direction
        return node.point_to_angle(node.robot.center, heading_point)

    def create_new_heading(self, conn_to_node_heading, conn_to_node_side, conn_from_node_side):
        if conn_to_node_side == "front_side":
            tmp_heading = conn_to_node_heading
        elif conn_to_node_side == "left_side":
            tmp_heading = conn_to_node_heading - 90
        elif conn_to_node_side == "back_side":
            tmp_heading = conn_to_node_heading + 180
        elif conn_to_node_side == "right_side":
            tmp_heading = conn_to_node_heading + 90

        while tmp_heading < 0 or tmp_heading >= 360:
            if tmp_heading >= 360:
                tmp_heading -= 360
            elif tmp_heading < 0:
                tmp_heading += 360

        if conn_from_node_side == "front_side":
            heading = tmp_heading - 180
        elif conn_from_node_side == "right_side":
            heading = tmp_heading + 90
        elif conn_from_node_side == "back_side":
            heading = tmp_heading
        elif conn_from_node_side == "left_side":
            heading = tmp_heading - 90

        while heading < 0 or heading >= 360:
            if heading >= 360:
                heading -= 360
            elif heading < 0:
                heading += 360

        return heading

    def create_new_position(self, conn_to_node_side, conn_to_point,
                            conn_to_node_center, conn_from_side, conn_to_node_id):
        left_right_sides = ["left_side", "right_side"]
        dx = conn_to_point.x - conn_to_node_center.x
        dy = conn_to_point.y - conn_to_node_center.y
        if conn_to_node_side in left_right_sides and conn_from_side in left_right_sides:
            x = conn_to_node_center.x + dx * 2.03
            y = conn_to_node_center.y + dy * 2.03
            return Point(x, y)
        else:
            conn_to_robot = self.nodes_dict[conn_to_node_id].robot
            x_sign = dx / abs(dx) if dx else 1
            y_sign = dy / abs(dy) if dy else 1

            side = "left_side" if conn_from_side in left_right_sides else "front_side"
            side_point = self.get_robot_side_position(conn_to_robot, side)

            ddy = abs(side_point.x - conn_to_node_center.x) * y_sign
            ddx = abs(side_point.y - conn_to_node_center.y) * x_sign

            x = conn_to_point.x + ddx * 1.2
            y = conn_to_point.y + ddy * 1.2

            return Point(x, y)

    def get_robot_side_position(self, robot, side):
        if side == "center":
            return robot.center
        elif side == "front_side":
            return robot.front_side
        elif side == "back_side":
            return robot.back_side
        elif side == "left_side":
            return robot.left_side
        elif side == "right_side":
            return robot.right_side
        else:
            return None

class TriModActionsHandler(object):
    def __init__(self, left_robot, right_robot, center_robot, style="trimod"):
        self.left_robot = left_robot
        self.right_robot = right_robot
        self.center_robot = center_robot
        self.start_action_time = None
        self.action_time = 7
        self.actions_list = [self.create_action(angle) for angle in [0, 90, 180, -90]]

        self.style = style

    def do_actions(self):
        # pass
        for action in self.actions_list:
            print("ACTIONS!!!!!!!!!!!!!!!")
            if self.start_action_time is None:
                self.init_start_time()
            # for _ in range(3):
            action()
            while time() - self.start_action_time <= self.action_time:
                pass
            self.stop_robots()
            self.start_action_time = None

    def init_actions_publishers(self, publishers_dict):
        if self.left_robot is not None:
            self.left_robot_publisher = publishers_dict[self.left_robot.id]
        else:
            self.left_robot_publisher = None
        if self.right_robot is not None:
            self.right_robot_publisher = publishers_dict[self.right_robot.id]
        else:
            self.right_robot_publisher = None
        self.center_robot_publisher = publishers_dict[self.center_robot.id]

    def init_start_time(self):
        self.start_action_time = time()

    def stop_robots(self):
        stop_msg = self.prepare_stop_msg()
        self.center_robot_publisher.publish(stop_msg)
        if self.right_robot_publisher is not None:
            self.right_robot_publisher.publish(stop_msg)
        if self.left_robot_publisher is not None:
            self.left_robot_publisher.publish(stop_msg)

    def create_action(self, center_robot_movement_angle):
        def action():
            print("action)))))!!!!!!!")
            center_robot_msg, l_side_robot_msg, r_side_robot_msg = self.prepare_action_msgs(center_robot_movement_angle)
            for _ in range(10):
                self.center_robot_publisher.publish(center_robot_msg)
            for _ in range(10):
                if self.right_robot_publisher is not None:
                   self.right_robot_publisher.publish(r_side_robot_msg)
            for _ in range(10):
                if self.left_robot_publisher is not None:
                    self.left_robot_publisher.publish(l_side_robot_msg)
        return action

    def prepare_action_msgs(self, center_robot_movement_angle):
        center_angle = angle_to_180(center_robot_movement_angle)
        if self.style == "trimod":
            l_sides_angle = angle_to_180(center_angle + 90)
            r_sides_angle = angle_to_180(center_angle + 90)
        else:
            sides_angle, l_sides_angle, r_sides_angle = center_angle, center_angle, center_angle
        center_robot_msg = self.prepare_movement_msg(center_angle, self.action_time)
        l_side_robot_msg = self.prepare_movement_msg(l_sides_angle, self.action_time)
        r_side_robot_msg = self.prepare_movement_msg(r_sides_angle, self.action_time)

        return center_robot_msg, l_side_robot_msg, r_side_robot_msg

    def prepare_movement_msg(self, angle, time):
        msg = ActionAtTime()
        msg.time = time*1000
        msg.angle = angle
        msg.movement = True
        msg.rotation = False
        return msg

    def prepare_stop_msg(self):
        msg = ActionAtTime()
        msg.time = 10
        msg.angle = 0
        msg.movement = False
        msg.rotation = False
        return msg


class Solution:
    def permute(self, nums: List[int]) -> List[List[int]]:
        finalOutput= []

        if (len(nums)==1):
            return [nums.copy()]

        for i in range(len(nums)):
            n=nums.pop(0)
            perms = self.permute(nums)

            for perm in perms:
                perm.append(n)

            finalOutput.extend(perms)
            nums.append(n)

        return finalOutput


#!/usr/bin/env python3

"""
Usage: ./back_nt.py new_blast_output.fa aa_alignment.out week5_query.fa

"""

import sys
from fasta import FASTAReader
import numpy as np
import matplotlib.pyplot as plt
import math

dna_reader = FASTAReader(open(sys.argv[1]))
dna_sequence = []
blast_dnaid = []
for ident, sequence in dna_reader:
   dna_sequence.append(sequence)
   blast_dnaid.append(ident)

# dna = dna_sequence[0]
# print(len(dna))

protein_reader = FASTAReader(open(sys.argv[2]))
protein_sequence = []

for ident, sequence in protein_reader:
   protein_sequence.append(sequence)

# print(len(protein_sequence))

l = len(protein_sequence)
aligned_dna = {}
# for i in range(len(protein_sequence)):
for i in range(len(protein_sequence)):
    dna = dna_sequence[i]
    gap_dna = ""
    count = 0
    for num, a in enumerate(protein_sequence[i]):
        if a == "-":
            gap_dna = gap_dna + "---"
            # print("***")
            # print(a)

        else:
            gap_dna = gap_dna + dna[count*3:count*3+3]
            count += 1
    # print(len(gap_dna))
    aligned_dna[blast_dnaid[i]] = gap_dna
    # print(len(dna))

# print(aligned_dna)

protein_id = {}

for i in range(len(protein_sequence)):
    protein_id[blast_dnaid[i]] = protein_sequence[i]

# print(protein_id)
# print(len(protein_id["M12294"]))
# print(len(protein_id["AY532665"]))
# print(len(aligned_dna["M12294"]))
# print(len(aligned_dna["AY532665"]))

total_number = len(protein_sequence)
length_aa = int(len(aligned_dna["M12294"])/3)

dS = [0] * length_aa
dN = [0] * length_aa

for i in range(0, len(aligned_dna["M12294"]), 3):
    q_codon = aligned_dna["M12294"][i:i+3]
    for j in range(1, 102):
        seq_id = blast_dnaid[j]
        dna_seq = aligned_dna[seq_id]
        # print(seq_id)
        seq_codon = dna_seq[i:i+3]
        if q_codon != seq_codon:
            q_aa = protein_id["M12294"][int(i/3)]
            seq_aa = protein_id[seq_id][int(i/3)]
            if q_aa == seq_aa:
                dS[int(i/3)] += 1
            if q_aa != seq_aa:
                dN[int(i/3)] += 1

    ### blast_dnaid[102]=blast_dnaid[103]=KJ883347, whose alignment is 'toxic' and very different from query.
    ### So I skipped that.

    for j in range(104, total_number):
        seq_id = blast_dnaid[j]
        dna_seq = aligned_dna[seq_id]
        # print(seq_id)
        seq_codon = dna_seq[i:i+3]
        if q_codon != seq_codon:
            q_aa = protein_id["M12294"][int(i/3)]
            seq_aa = protein_id[seq_id][int(i/3)]
            if q_aa == seq_aa:
                dS[int(i/3)] += 1
            if q_aa != seq_aa:
                dN[int(i/3)] += 1

# print(aligned_dna["HM756673"])
# print(dS)
# print(dN)

# print(protein_id["M12294"])
# print("...")
# print(protein_id["KJ883347"])

dD_values = []
ratio = []
for i in range(length_aa):
    dD_values.append(dN[i] - dS[i])
    ratio.append((dN[i]+1)/(dS[i]+1))

# print(dD_values)
# print(ratio)

mean = np.mean(dD_values)
std = np.std(dD_values)

# print(len(dD_values))

z = []
for i in range(len(dD_values)):
    z.append(abs((dD_values[i]-mean)/std))

# print(z)

print("The mean of all D values is " + str(mean))
print("The standard deviation of all D values is " + str(std))
print("For Z-test, alpha = 0.05, critical z value = 1.645")

fig, ax = plt.subplots()
for i in range(len(ratio)):
    if z[i] < 1.645:
        ax.scatter(i, ratio[i], alpha = 0.3, color = "blue", s = 3)
    else:
        ax.scatter(i, ratio[i], alpha = 0.3, color = "red", s = 3)
ax.plot([0,len(ratio)],[1,1], color = "orange", label = "ratio = 1")
ax.set_title("dN/dS Ratio Distribution")
ax.set_ylabel("dN/dS")
ax.set_xlabel("Position within Query Amino Acid Sequence")
fig.savefig("dN_to_dS_distribution.png")
plt.close(fig)


import argparse
import json
from elasticsearch import Elasticsearch
from elasticsearch.helpers import scan
from utils import Config

def search_papers(search_phrase):
  es = Elasticsearch([{
    'host': Config.host,
    'port': Config.port,
    'use_ssl': True,
    'http_auth': Config.http_auth
  }])

  query = {
    "query": {
      "match_phrase": {
        "_all": search_phrase
      }
    },
    "_source": {
          "includes": [ "metadata" ]
    },
  }

  search_scanner = scan(es, query=query, index='papers', doc_type='paper')
  hits = [hit['_source']['metadata'] for hit in search_scanner]
  with open(search_phrase + '-results', 'w') as f:
    json.dump(hits, f)

  print('Found %d matches.' % (len(hits)))

if __name__ == "__main__":
  parser = argparse.ArgumentParser()
  parser.add_argument('--search-phrase', type=str, required=True, help='string phrase to search for')
  args = parser.parse_args()
  search_papers(args.search_phrase)


#!/usr/bin/env python3

'''
Basic IPv4 router (static routing) in Python.
'''

import sys
import os
import time

from switchyard.lib.packet.util import *
from switchyard.lib.userlib import *

class Router(object):
    def __init__(self, net):
        self.net = net
        #retrieve interfaces for router
        self.my_interfaces = self.net.interfaces()
        self.myArpTable = ArpTable()  #initialize ARP table

    def router_main(self):
        '''
        Main method for router; we stay in a loop in this method, receiving
        packets until the end of time.
        '''
        while True:
            gotpkt = True
            try:
                timestamp,input_port,packet = self.net.recv_packet(timeout=1.0)
            except NoPackets:
                log_debug("No packets available in recv_packet")
                gotpkt = False
            except Shutdown:
                log_debug("Got shutdown signal")
                break

            if gotpkt:
                log_debug("Got a packet: {}".format(str(packet)))
                log_debug (" *** Received packet {} on {}".format(packet, input_port))

                #check if it is an ARP request
                arp_header = packet.get_header(Arp)

                #if not an arp packet, drop it
                if arp_header is None:
                    log_debug(" *** Not an ARP packet, drop it.")
                    continue

                #retrieve targetprotoaddr from the ARP header
                targetprotoaddr = arp_header.targetprotoaddr
                log_debug(" *** Target IP address is: {}".format(targetprotoaddr))

                #retrieve interface for target address
                try:
                    interface_in_router = self.net.interface_by_ipaddr(targetprotoaddr)
                except KeyError:
                    log_debug("Target IP address is not in router, dropping packet.")
                    continue

                log_debug(" *** Target interface is: {} - assigned to an interface in this router.".format(interface_in_router))

                #if this is an ARP request and we have a
                if arp_header.operation == ArpOperation.Request:
                    log_debug(" *** Received an ARP request.")

                    #create and send an ARP reply message
                    #reply leaves on interface in router, sending back to ARP request sender
                    arp_reply_packet = create_ip_arp_reply(interface_in_router.ethaddr, arp_header.senderhwaddr, arp_header.targetprotoaddr, arp_header.senderprotoaddr)
                    log_debug(" *** Created ARP reply packet back on input port: {}".format(input_port))
                    self.net.send_packet(input_port,arp_reply_packet)
                    log_debug(" *** Sent ARP reply packet.")
                    continue

                #if this is an ARP reply and we have a matching interface  - update table
                elif arp_header.operation == ArpOperation.Reply:
                    log_debug(" *** Received an ARP reply.")

                    #store a mapping of the ARP in your router
                    self.myArpTable.updateTable(arp_header.senderprotoaddr, arp_header.senderhwaddr,time.time())

def main(net):
    '''
    Main entry point for router.  Just create Router
    object and get it going.
    '''
    r = Router(net)
    r.router_main()
    net.shutdown()

#ArpTable objects will store a list of ArpTableEntry objects to support ARP tables
class ArpTable:
    def __init__(self):
        self.tableEntries = list()

    #retrieve MAC address given an IP address
    def getMac(self, ipAddrToCheck):
        for tableEntry in self.tableEntries:
            if tableEntry.ipAddr == ipAddrToCheck: return tableEntry.macAddr

    #upon receiving new mapping, add to ARP table
    def updateTable(self,ipAddrToAdd, macAddrToAdd, timestamp):

        #if the ipAddr is in the table, update MAC and return
        for tableEntry in self.tableEntries:
            if tableEntry.ipAddr == ipAddrToAdd:
                tableEntry.macAddr = macAddrToAdd
                return

        #otherwise add the IP / MAC address pair to the table
        self.tableEntries.append(ArpTableEntry(ipAddrToAdd, macAddrToAdd,timestamp))

#ArpTableEntry objects will store IP address (protoaddr) and MAC (senderhwaddraddr) and timestamp
class ArpTableEntry:
    def __init__(self,ipAddr,macAddr,timestamp):
        self.ipAddr = ipAddr
        self.macAddr = macAddr
        return


import numpy as np
import pandas as pd
target = np.array(["abc",  "aaa" , "ccc" , "ddd" , "abc"])
# target = np.array([1,  1, 2, 2, 0])
print(target)

def onehot_encode(target):
    num_arr = np.unique(target, axis=0)
    num = num_arr.tolist()
    result_list = []
    for tar in target:
        n_arr = np.zeros(num_arr.shape[0])
        n_arr[num.index(tar)] = 1
        result_list.append(n_arr)

    return np.array(result_list), num

one_hot, class_list = onehot_encode(target)
print(one_hot)
print(class_list)

def onehot_decode(one_hot, class_list):
    result_list = []
    for tar in one_hot:
        tar_index = tar.argmax()
        result_list.append(class_list[tar_index])
    return result_list

data = onehot_decode(one_hot, class_list)
print(data)

# encoding = np.eye(num)[target]
# print(encoding)


import os
import numpy as np
from keras.models import Sequential
from keras.layers import Dense

from Library.General import DataThings as DT
from Library.Network import KerasNetwork as KN

class When(KN.KerasNetwork):
    """ classification neural network """

    def __init__(self, test):
        """ """

        # load network
        self.test = test
        KN.KerasNetwork.__init__(self, '{} when'.format(test.name),
                                 test.when_network_path)
        self.labels = ['wait', 'action', 'end']
        self.load_network()

    ### NETWORK ###

    def create_network(self, n_hidden=64, n_layers=3):
        """ create ANN reward nertwork with keras """
        # first layer
        network = Sequential()
        network.add(Dense(n_hidden, activation='relu',
                          input_shape=self.test.mid_size))
        # hidden layers
        for _ in range(n_layers - 1):
            network.add(Dense(n_hidden, activation='relu'))
        # last layer
        network.add(Dense(len(self.labels), activation='softmax'))
        network.compile(loss='categorical_crossentropy', optimizer='adam')
        self.network = network

    ### TRAIN - ONLINE ###

    def train_network(self, data, labels, bs=2, ep=50, cutoff=1e-4):
        """ action = (button, x, y, time) other """
        # format input data
        print('\nTraining when')
        labels = [DT.new_label(self.labels.index(lab), len(self.labels))
                  for lab in labels if lab in self.labels]
        labels = np.array(labels)
        print(' - data shape: {}'.format(data.shape))
        print(' - labels shape: {}'.format(labels.shape))
        print(labels)
        # loop until under cutoff
        count, cost = 0, 100
        while cost > cutoff and count < 30:
            self.network.fit(data, labels, batch_size=bs, epochs=ep, verbose=0)
            cost = self.network.evaluate(data, labels, batch_size=bs, verbose=0)
            print(' - cost {}: {}'.format(count, cost))
            count += 1


"""
Created on 30 Dec 2016

@author: Bruno Beloff (bruno.beloff@southcoastscience.com)

https://www.u-blox.com/en/product/pam-7q-module

example sentence set:
$GPGLL,5049.38388,N,00007.37172,W,113111.00,A,A*7E
$GPRMC,113112.00,A,5049.38381,N,00007.37173,W,0.017,,201119,,,A*60
$GPVTG,,T,,M,0.017,N,0.032,K,A*24
$GPGGA,113112.00,5049.38381,N,00007.37173,W,1,04,2.48,36.5,M,45.4,M,,*7F
$GPGSA,A,3,07,19,06,23,,,,,,,,,4.22,2.48,3.41*06
$GPGSV,2,1,08,06,66,247,39,07,24,164,47,09,,,33,17,04,214,39*48
$GPGSV,2,2,08,19,15,227,50,23,57,066,33,24,,,30,26,,,29*76
"""

from scs_core.position.nmea.gpgga import GPGGA
from scs_core.position.nmea.gpgll import GPGLL
from scs_core.position.nmea.gpgsa import GPGSA
from scs_core.position.nmea.gpgsv import GPGSV
from scs_core.position.nmea.gprmc import GPRMC
from scs_core.position.nmea.gpvtg import GPVTG
from scs_core.position.nmea.nmea_report import NMEAReport

from scs_dfe.gps.gps import GPS

# --------------------------------------------------------------------------------------------------------------------

class PAM7Q(GPS):
    """
    u-blox 7 GPS Antenna Module
    """

    SOURCE =                    "PAM7Q"

    # ----------------------------------------------------------------------------------------------------------------

    __BAUD_RATE =               9600
    __BOOT_DELAY =              0.500           # seconds

    __SERIAL_LOCK_TIMEOUT =     6.0
    __SERIAL_COMMS_TIMEOUT =    5.0

    __EOL =                     "\r\n"

    # ----------------------------------------------------------------------------------------------------------------

    @classmethod
    def baud_rate(cls):
        return cls.__BAUD_RATE

    @classmethod
    def boot_time(cls):
        return cls.__BOOT_DELAY

    @classmethod
    def serial_lock_timeout(cls):
        return cls.__SERIAL_LOCK_TIMEOUT

    @classmethod
    def serial_comms_timeout(cls):
        return cls.__SERIAL_COMMS_TIMEOUT

    # ----------------------------------------------------------------------------------------------------------------

    def __init__(self, interface, uart):
        super().__init__(interface, uart)

    # ----------------------------------------------------------------------------------------------------------------

    def report(self, message_class):
        self._serial.flush_input()

        while True:
            try:
                sentence = self.__sentence()
            except TimeoutError:
                return None

            if sentence.message_id in message_class.MESSAGE_IDS:
                return message_class.construct(sentence)

    def report_all(self):
        self._serial.flush_input()

        reports = []

        # find start...
        while True:
            sentence = self.__sentence()

            if sentence.message_id in GPGLL.MESSAGE_IDS:
                break

        reports.append(GPGLL.construct(sentence))

        reports.append(GPRMC.construct(self.__sentence()))
        reports.append(GPVTG.construct(self.__sentence()))
        reports.append(GPGGA.construct(self.__sentence()))
        reports.append(GPGSA.construct(self.__sentence()))

        # find end...
        while True:
            sentence = self.__sentence()

            if sentence.message_id not in GPGSV.MESSAGE_IDS:
                break

            reports.append(GPGSV.construct(sentence))

        return reports

    # ----------------------------------------------------------------------------------------------------------------

    def __sentence(self):
        while True:
            try:
                line = self._serial.read_line(eol=self.__EOL, timeout=self.__SERIAL_COMMS_TIMEOUT)
                self._logger.debug("sentence:[%s]" % line)

                return NMEAReport.construct(line)

            except (IndexError, UnicodeDecodeError, ValueError):
                continue


# -*- coding: utf-8 -*-

import base64
import json
import select
import socket
import struct
import sys
import threading
import time
import uuid

import requests

# HOST = "imnode.gobelieve.io"
HOST = "127.0.0.1"
PORT = 23000

#command
MSG_HEARTBEAT = 1
#MSG_AUTH = 2
MSG_AUTH_STATUS = 3
MSG_IM = 4
MSG_ACK = 5
MSG_RST = 6
MSG_GROUP_NOTIFICATION = 7
MSG_GROUP_IM = 8
MSG_PEER_ACK = 9
MSG_INPUTING = 10
MSG_SUBSCRIBE_ONLINE_STATE = 11
MSG_ONLINE_STATE = 12
MSG_PING = 13
MSG_PONG = 14
MSG_AUTH_TOKEN = 15
MSG_LOGIN_POINT = 16
MSG_RT = 17
MSG_SYSTEM = 21

#platform
PLATFORM_IOS = 1
PLATFORM_ANDROID = 2
PLATFORM_WEB = 3
PLATFORM_SERVER = 4

PROTOCOL_VERSION = 1

class AuthenticationToken:
    def __init__(self):
        self.token = ""
        self.platform_id = PLATFORM_SERVER
        self.device_id = ""

class IMMessage:
    def __init__(self):
        self.sender = 0
        self.receiver = 0
        self.timestamp = 0
        self.msgid = 0
        self.content = ""
class RTMessage:
    def __init__(self):
        self.sender = 0
        self.receiver = 0
        self.content = ""

def send_message(cmd, seq, msg, sock):
    if cmd == MSG_AUTH_TOKEN:
        b = struct.pack("!BB", msg.platform_id, len(msg.token)) + msg.token + struct.pack("!B", len(msg.device_id)) + msg.device_id
        length = len(b)
        h = struct.pack("!iibbbb", length, seq, cmd, PROTOCOL_VERSION, 0, 0)
        sock.sendall(h+b)
    elif cmd == MSG_IM or cmd == MSG_GROUP_IM:
        length = 24 + len(msg.content)
        h = struct.pack("!iibbbb", length, seq, cmd, PROTOCOL_VERSION, 0, 0)
        b = struct.pack("!qqii", msg.sender, msg.receiver, msg.timestamp, msg.msgid)
        sock.sendall(h+b+msg.content)
    elif cmd == MSG_RT:
        length = 16 + len(msg.content)
        h = struct.pack("!iibbbb", length, seq, cmd, PROTOCOL_VERSION, 0, 0)
        b = struct.pack("!qq", msg.sender, msg.receiver)
        sock.sendall(h+b+msg.content)
    elif cmd == MSG_ACK:
        h = struct.pack("!iibbbb", 4, seq, cmd, PROTOCOL_VERSION, 0, 0)
        b = struct.pack("!i", msg)
        sock.sendall(h + b)
    elif cmd == MSG_INPUTING:
        sender, receiver = msg
        h = struct.pack("!iibbbb", 16, seq, cmd, PROTOCOL_VERSION, 0, 0)
        b = struct.pack("!qq", sender, receiver)
        sock.sendall(h + b)
    elif cmd == MSG_PING:
        h = struct.pack("!iibbbb", 0, seq, cmd, PROTOCOL_VERSION, 0, 0)
        sock.sendall(h)
    else:
        print("eeeeee")

def recv_message(sock):
    buf = sock.recv(12)
    if len(buf) != 12:
        return 0, 0, None
    length, seq, cmd = struct.unpack("!iib", buf[:9])

    if length == 0:
        return cmd, seq, None

    content = sock.recv(length)
    if len(content) != length:
        return 0, 0, None

    if cmd == MSG_AUTH_STATUS:
        status, = struct.unpack("!i", content)
        return cmd, seq, status
    elif cmd == MSG_LOGIN_POINT:
        up_timestamp, platform_id = struct.unpack("!ib", content[:5])
        device_id = content[5:]
        return cmd, seq, (up_timestamp, platform_id, device_id)
    elif cmd == MSG_IM or cmd == MSG_GROUP_IM:
        im = IMMessage()
        im.sender, im.receiver, im.timestamp, im.msgid = struct.unpack("!qqii", content[:24])
        im.content = content[24:]
        return cmd, seq, im
    elif cmd == MSG_RT:
        rt = RTMessage()
        rt.sender, rt.receiver, = struct.unpack("!qq", content[:16])
        rt.content = content[16:]
        return cmd, seq, rt
    elif cmd == MSG_ACK:
        ack, = struct.unpack("!i", content)
        return cmd, seq, ack
    elif cmd == MSG_INPUTING:
        sender, receiver = struct.unpack("!qq", content)
        return cmd, seq, (sender, receiver)
    elif cmd == MSG_PONG:
        return cmd, seq, None
    else:
        return cmd, seq, content

class Client(object):
    def __init__(self):
        self.seq = 0
        self.sock = None

    def connect_server(self, device_id, token, host=None):
        if host is not None:
            address = (host, PORT)
        else:
            address = (HOST, PORT)
        sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
        sock.connect(address)
        auth = AuthenticationToken()
        auth.token = token
        auth.device_id = device_id
        self.seq = self.seq + 1
        send_message(MSG_AUTH_TOKEN, self.seq, auth, sock)
        cmd, _, msg = recv_message(sock)
        if cmd != MSG_AUTH_STATUS or msg != 0:
            return False

        self.sock = sock
        return True

    def close(self):
        self.sock.close()

    def ack_message(self, s):
        self.seq += 1
        send_message(MSG_ACK, self.seq, s, self.sock)

    def recv_message(self):
        while True:
            rlist, _, xlist = select.select([self.sock], [], [self.sock], 60)
            if not rlist and not xlist:
                #timeout
                self.seq += 1
                print("ping...")
                send_message(MSG_PING, self.seq, None, self.sock)
                continue
            if xlist:
                return 0, 0, None
            if rlist:
                cmd, s, m = recv_message(self.sock)
                return cmd, s, m

DEVICE_ID = "ec452582-a7a9-11e5-87d3-34363bd464b2"
if __name__ == "__main__":
    #调用app自身的服务器获取连接im服务必须的access token
    url = "http://demo.gobelieve.io/auth/token";
    headers = {'Content-Type': 'application/json; charset=UTF-8'}
    uid = 1
    name = "测试用户%d"%uid
    obj = {"uid":uid, "user_name":name}
    resp = requests.post(url, data=json.dumps(obj), headers=headers)
    if resp.status_code != 200:
        sys.exit(1)
    obj = json.loads(resp.text)
    token = obj["token"]
    print("token:", token)

    while True:
        try:
            client = Client()
            r = client.connect_server(DEVICE_ID, token)
            if not r:
                continue
            while True:
                print("recv message...")
                cmd, s, m = client.recv_message()
                #socket disconnect
                if cmd == 0 and s == 0 and m is None:
                    print("socket disconnect")
                    break

                print("cmd:", cmd)
                if cmd == MSG_IM:
                    #handle im
                    print("im:", m.sender, m.receiver, m.timestamp, m.content)
                    client.ack_message(s)
                elif cmd == MSG_GROUP_IM:
                    #handle group im
                    print("group im:", m.sender, m.receiver, m.timestamp, m.content)
                    client.ack_message(s)
                elif cmd == MSG_SYSTEM:
                    #handle system
                    print("system:", m)
                    client.ack_message(s)
                elif cmd == MSG_PONG:
                    print("pong...")
                    continue
                else:
                    print("unknow message:", cmd)
                    continue

        except Exception as e:
            print("exception:", e)
            time.sleep(1)
            continue


from django.urls import path, re_path
from .views import *

app_name = 'booktest'

urlpatterns = [
    path('', index, name='index'),
    # re_path("^(\d+)$", detail, name='detail'),
    re_path("^(\d+)$", show, name='show'),
    path("get_test1/", get_test1),
    path("get_test2/", get_test2),
    path("get_test3/", get_test3),
    path("post_test1/", post_test1),
    path("post_test2/", post_test2),
    path("cookie_test/", cookie_test),
    path("red_test1/", red_test1),
    path("red_test2/", red_test2),
    path("session1/", session1),
    path("session2/", session2),
    path("session2_handle/", session2_handle),
    path("session3/", session3),
    path("template_test/", template_test),
    re_path("^show_hero_detail_(\d+)$", show_hero_detail, name='show_hero_detail'),
    path("child_index/", child_index),
    re_path("^user_(\d+)$", user, name='user'),
    path("html_test/", html_test),
    re_path("^csrf_(\d+)$", csrf_test, name='csrf_test'),
    re_path("^hero_list/(\d*)/?$", hero_list),
    path("cache1/", cache1),
    path("cache2/", cache2),
    path("cache3/", cache3),
    path("search_hero_index/", search_hero_index),
    path("celery_index/", celery_index),
]


#!/usr/bin/env python3

import sqlite3
import argparse
import os

db_filename = 'data.db'
db_table = 'urls'
file_dir = 'images/'

def add_to_db(url=None, tag=''):
    conn = sqlite3.connect(db_filename)
    c = conn.cursor()
    c.execute('''CREATE TABLE IF NOT EXISTS urls (url text, tag text)''')
    c.execute("INSERT INTO " + db_table + " ('url', 'tag') VALUES (?,?)", (url,tag))
    conn.commit()
    conn.close()

def del_url(url=None, tag=''):
    conn = sqlite3.connect(db_filename)
    c = conn.cursor()
    c.execute("DELETE FROM " + db_table + " where url = ?", (url,))
    try:
        fpath = os.path.join(file_dir, os.path.basename(url))
        print("trying to delete ",fpath," ...")
        os.remove(fpath)
    except:
        pass
    conn.commit()
    conn.close()

def run():
   parser = argparse.ArgumentParser()
   parser.add_argument("--add", "-a", help="add an URL")
   parser.add_argument("--tag", "-t", help="add a tag", default='funny')
   parser.add_argument("--delete", "-d", help="delete an URL")
   args = parser.parse_args()

   if args.add:
      print("adding URL...")
      add_to_db(args.add, args.tag)
   if args.delete:
      print("deleting URLs")
      del_url(args.delete, args.tag)

if __name__ == '__main__':
    run()


import os
from onconet.datasets.factory import RegisterDataset
from onconet.datasets.abstract_onco_dataset import Abstract_Onco_Dataset
import onconet.utils.generic

METADATA_FILENAME = "mgh-mammo-metadata-final-correct-breast-crop-2560.json"
SUMMARY_MSG = "Contructed HRL {} {} dataset with {} records, and the following class balance \n {}"

class Abstract_HRL_Dataset(Abstract_Onco_Dataset):

    def create_dataset(self, split_group, img_dir):
        """
        Creating the dataset from the paths and labels in the json.

        :split_group: - ['train'|'dev'|'test'].
        :img_dir: - The path to the dir containing the images.

        """
        dataset = []
        class_balance = {}
        for row in self.metadata_json:

            def fits_criteria(row):
                group_valid = 'split_group' in row and row['split_group'] == split_group
                label_valid = self.check_label(row)
                path_valid = 'thumbnail_path' in row and os.path.exists(
                    os.path.join(img_dir, row['thumbnail_path']))
                return group_valid and label_valid and path_valid

            if fits_criteria(row):
                label = self.get_label(row)
                if label not in class_balance:
                    class_balance[label] = 0
                class_balance[label] += 1
                dataset.append({
                    'path':
                    os.path.join(img_dir, row['thumbnail_path']),
                    'y':
                    label,
                    'additional': {}
                })
        class_balance = onconet.utils.generic.normalize_dictionary(class_balance)
        print(
            SUMMARY_MSG.format(self.task, split_group, len(dataset), class_balance))
        return dataset

    @property
    def METADATA_FILENAME(self):
        return METADATA_FILENAME

@RegisterDataset("hrl_full_density")
class HRL_Full_Density(Abstract_HRL_Dataset):
    """
    Pytorch Dataset object for the full density task on the HRL dataset.
    Full density is defined as  discrimanting between:
        0- fatty
        1- cattered fibroglandular
        2- heterogeneously dense
        3- dense
    on patient mammograms.
    """

    def __init__(self, args, transformer, split_group):
        '''
        params: args - config.
        params: transformer - A transformer object, takes in a PIL image, performs some transforms and returns a Tensor
        params: split_group - ['train'|'dev'|'test'].

        constructs: standard pytorch Dataset obj,which can be fed in a DataLoader for batching
        '''
        super(HRL_Full_Density, self).__init__(args, transformer, split_group)

    @staticmethod
    def set_args(args):
        args.num_classes = 4

    @property
    def task(self):
        return "Full Density"

    def check_label(self, row):
        return 'DENSITY' in row and row['DENSITY'] in [1, 2, 3, 4]

    def get_label(self, row):
        return row['DENSITY'] - 1

@RegisterDataset("hrl_binary_density")
class HRL_Binary_Density(Abstract_HRL_Dataset):
    """
    Pytorch Dataset object for the binary density task on the HRL dataset.
    Binary density is defined as  discrimanting between:
        0- low density (i.e fatty or scattered fibroglandular)
        1- high density (i.e hetrogenously dense or dense)
    on patient mammograms.
    """

    def __init__(self, args, transformer, split_group):
        '''
        params: args - config.
        params: transformer - A transformer object, takes in a PIL image, performs some transforms and returns a Tensor
        params: split_group - ['train'|'dev'|'test'].

        constructs: standard pytorch Dataset obj,which can be fed in a DataLoader for batching
        '''
        super(HRL_Binary_Density, self).__init__(args, transformer,
                                                 split_group)

    @staticmethod
    def set_args(args):
        args.num_classes = 2

    @property
    def task(self):
        return "Binary Density"

    def check_label(self, row):
        return 'DENSITY' in row and row['DENSITY'] in [1, 2, 3, 4]

    def get_label(self, row):
        label_map = {1: 0, 2: 0, 3: 1, 4: 1}
        return label_map[row['DENSITY']]


# -*- coding: utf-8 -*-
from Plugins.Extensions.IPTVPlayer.tools.iptvtools import printDBG
from Plugins.Extensions.IPTVPlayer.libs import ph
from Plugins.Extensions.IPTVPlayer.tsiplayer.libs.tstools import TSCBaseHostClass,gethostname,tscolor
from Plugins.Extensions.IPTVPlayer.libs.e2ijson import loads as json_loads
from Plugins.Extensions.IPTVPlayer.tools.iptvtypes import strwithmeta
from Plugins.Extensions.IPTVPlayer.libs.urlparserhelper import getDirectM3U8Playlist
from Plugins.Extensions.IPTVPlayer.libs.urlparserhelper import unpackJSPlayerParams, SAWLIVETV_decryptPlayerParams
###################################################

import re
import base64
import urllib

def getinfo():
	info_={}
	info_['name']='RobinDesDroits.Me'
	info_['version']='1.0 20/07/2019'
	info_['dev']='RGYSoft'
	info_['cat_id']='110'
	info_['desc']='Sports Replay VF'
	info_['icon']='https://i.ibb.co/4f0fLpP/bs1kgdez.png'
	info_['recherche_all']='1'
	info_['update']='New Host'
	return info_

class TSIPHost(TSCBaseHostClass):
	def __init__(self):
		TSCBaseHostClass.__init__(self,{'cookie':'robindesdroits.cookie'})
		self.USER_AGENT = 'Mozilla/5.0 (Windows NT 6.1; WOW64; rv:40.0) Gecko/20100101 Firefox/40.0'
		self.MAIN_URL = 'http://robindesdroits.me'
		self.HTTP_HEADER = {'User-Agent': self.USER_AGENT, 'DNT':'1', 'Accept': 'text/html', 'Accept-Encoding':'gzip, deflate', 'Referer':self.getMainUrl(), 'Origin':self.getMainUrl()}
		self.defaultParams = {'with_metadata':True,'header':self.HTTP_HEADER, 'use_cookie': True, 'load_cookie': True, 'save_cookie': True, 'cookiefile': self.COOKIE_FILE}

	def getPage(self,baseUrl, addParams = {}, post_data = None):
		while True:
			if addParams == {}: addParams = dict(self.defaultParams)
			origBaseUrl = baseUrl
			baseUrl = self.cm.iriToUri(baseUrl)
			addParams['cloudflare_params'] = {'cookie_file':self.COOKIE_FILE, 'User-Agent':self.USER_AGENT}
			sts, data = self.cm.getPageCFProtection(baseUrl, addParams, post_data)
			printDBG(str(sts))
			if sts and 'class="loading"' in data:
				GetIPTVSleep().Sleep(5)
				continue
			break
		return sts, data

	def showmenu0(self,cItem):
		hst='host2'
		img_=cItem['icon']
		Cat_TAB = [
					{'category':hst,'title': 'Nouveautés',       'mode':'30','url':self.MAIN_URL + '/derniers-uploads/'},
					{'category':hst,'title': 'Football',         'mode':'30','url':self.MAIN_URL + '/football/'},
					{'category':hst,'title': 'Rugby',            'mode':'30','url':self.MAIN_URL + '/rugby/'},
					{'category':hst,'title': 'Basketball',       'mode':'30','url':self.MAIN_URL + '/basketball/'},
					{'category':hst,'title': 'Sport Automobiles','mode':'30','url':self.MAIN_URL + '/sports-automobiles/'},
					{'category':hst,'title': 'Sport US',         'mode':'30','url':self.MAIN_URL + '/sports-us/'},
					{'category':hst,'title': 'Tennis',           'mode':'30','url':self.MAIN_URL + '/tennis/'},
					{'category':hst,'title': 'Handball',         'mode':'30','url':self.MAIN_URL + '/handball/'},
					{'category':hst,'title': 'Divers',           'mode':'30','url':self.MAIN_URL + '/divers/'},
					#{'category':hst,'title': 'By Genre',         'mode':'21','sub_mode':0},
					{'category':'search','title': _('Search'), 'search_item':True,'page':1,'hst':'tshost'},
					]
		self.listsTab(Cat_TAB, {'import':cItem['import'],'icon':img_})

	def showitms(self,cItem):
		url1=cItem['url']
		page=cItem.get('page',1)
		url1=url1+'page/'+str(page)+'/'
		sts, data = self.getPage(url1)
		printDBG('ddddddddddddddddddddddddddd'+data)
		sPattern = 'mh-loop-thumb">.*?href="(.*?)".*?url\(\'(.+?)\'.*?bookmark">(.+?)
'
		data_list = re.findall(sPattern, data, re.S)
		i=0
		for (url,image,titre) in data_list:
			i=i+1
			self.addVideo({'import':cItem['import'],'category' : 'host2','url': url,'title':ph.clean_html(titre),'desc':'','icon':image,'hst':'tshost','good_for_fav':True})
		if i>9:
			self.addDir({'import':cItem['import'],'title':tscolor('\c0000????')+'Page Suivante','page':page+1,'category' : 'host2','url':cItem['url'],'icon':cItem['icon'],'mode':'30'} )

	def SearchResult(self,str_ch,page,extra):
		url_=self.MAIN_URL+'/page/'+str(page)+'/?s='+str_ch
		sts, data = self.getPage(url_)
		if sts:
			data_list = re.findall('mh-loop-thumb">.*?href="(.*?)".*?url\(\'(.+?)\'.*?bookmark">(.+?)
', data, re.S)
			for (url,image,titre) in data_list:
				self.addVideo({'import':extra,'category' : 'host2','url': url,'title':ph.clean_html(titre),'desc':'','icon':image,'hst':'tshost','good_for_fav':True})

	def get_links(self,cItem):
		urlTab = []
		URL=cItem['url']
		sts, data = self.getPage(URL)
		data_list = re.findall('(?:<span.+?|)([^<]+).+?', data, re.S)
		for (url,host) in data_list:
			if ('ulti' not in host) and ('heberg' not in host):
				urlTab.append({'name':host.replace('–','-'), 'url':'hst#tshost#'+url, 'need_resolve':1})

		return urlTab

	def getVideos(self,videoUrl):
		urlTab = []
		sts, data = self.getPage(videoUrl)
		if sts:
			if '<meta http-equiv="refresh"' in data:
				data_list = re.findall('URL=(.*?)"', data, re.S)
				if data_list:
					videoUrl=data_list[0]
					sts, data = self.getPage(videoUrl)
			if 'AdF' in data:
				printDBG('ffffffffffff1'+data)
				sUrl = self.AdflyDecoder(videoUrl)
				if 'motheregarded' in sUrl:
					data_list = re.findall('href=(.+?)&dp_lp', sUrl, re.S)
					if data_list:
						sUrl = urllib.unquote(''.join(data_list[0])).decode('utf8')
				sts, data = self.getPage(sUrl)
			if sts:
				printDBG('ffffffffffff'+data)
				data_list = re.findall('', data, re.S)
				if data_list:
					uurl=data_list[0].replace('%3A',':').replace('%2F','/').replace('%0D','/')
					urlTab.append((uurl,'1'))
		return urlTab

	def AdflyDecoder(self,url):
		code=''
		sts, data = self.getPage(url)
		if sts:
			data_list = re.findall("var ysmm = '([^']+)'", data, re.S)
			if data_list:
				from base64 import b64decode
				code = data_list[0]
				A = ''
				B = ''
				#First pass
				for num in enumerate(code):
					if num % 2 == 0:
						A += code[num]
					else:
						B = code[num] + B
				code = A + B

				#Second pass
				m = 0
				code = list(code)
				while m < len(code) :
					if code[m].isdigit() :
						R = m + 1
						while R < len(code):
							if code[R].isdigit() :
								S = int(code[m]) ^ int(code[R])
								if (S < 10):
									code[m] = str(S)
								m = R
								R = len(code)
							R += 1
					m += 1
				code = ''.join(code)
				code = b64decode(code)
				code = code[16:]
				code = code[:-16]
		return code

	def start(self,cItem):
		mode=cItem.get('mode', None)
		if mode=='00':
			self.showmenu0(cItem)
		if mode=='20':
			self.showmenu1(cItem)
		if mode=='21':
			self.showmenu2(cItem)
		if mode=='30':
			self.showitms(cItem)
		if mode=='31':
			self.showelms(cItem)


# ****************************************************************
# Find check there exists a reponse objects exists for a petition
# and Cadmin
# ****************************************************************

from django import template
from mysite.models import *
from django.contrib.auth.models import User

register = template.Library()

def FindResponseTrue(user_value, petition_id):
    try:
        PetitionResponseFeedback.objects.get(
            user=User.objects.get(id=user_value),
            petition = Petition.objects.get(id=petition_id)
        )
        return True
    except:
        return False

register.filter(FindResponseTrue)


#Python3 code
"""
Roll no.: 170122014

2. Solve using trapezoidal rule (upto 4 decimal places)

f(x)=sqrt(x*x+1)
n=5
lower limit of integration = 0
upper limit of integration=1

Answer: 1.1501
"""
import math
#Trapezoidal Rule
n = 5   #given, n=5
a = 0   #lower limit = 0
b = 1   #upper limit =1
h = (b-a)/n

def f(x):
    return math.sqrt((pow(x, 2) + 1))

sum = 0
for i in range(1, n):
    sum = sum + f(a+i*h)
    #print('sum = ',sum)

term = ((h/2)*(f(a) + f(b) + (2*sum)))
print('Using Trapeziodal Rule the integral value, I =  {:.4f}'.format(term))
#print("f(a)= ", f(a), "f(b)= ", f(b), "h= ", h)


"""
copied from
https://github.com/raphaelauv/fastAPI-aiohttp-example/blob/master/src/fastAPI_aiohttp/fastAPI.py
"""

import aiohttp
import asyncio
import logging

from socket import AF_INET
from typing import Optional, Any

from fastapi.exceptions import HTTPException

from test.service.config import SIZE_POOL_AIOHTTP

logger = logging.getLogger('test.service')

class SingletonAiohttp:
    sem: Optional[asyncio.Semaphore] = None
    aiohttp_client: Optional[aiohttp.ClientSession] = None

    @classmethod
    def get_aiohttp_client(cls) -> aiohttp.ClientSession:
        if cls.aiohttp_client is None:
            timeout = aiohttp.ClientTimeout(total=2)
            connector = aiohttp.TCPConnector(family=AF_INET, limit_per_host=SIZE_POOL_AIOHTTP)
            cls.aiohttp_client = aiohttp.ClientSession(timeout=timeout, connector=connector)

        return cls.aiohttp_client

    @classmethod
    async def close_aiohttp_client(cls) -> None:
        if cls.aiohttp_client:
            await cls.aiohttp_client.close()
            cls.aiohttp_client = None

    @classmethod
    async def query_url(cls, url: str) -> Any:
        client = cls.get_aiohttp_client()

        try:
            async with client.get(url) as response:
                if response.status != 200:
                    return HTTPException(status_code=response.status, detail=str(await response.text()))

                json_result = await response.json()
        except (asyncio.exceptions.TimeoutError, asyncio.exceptions.CancelledError) as e:
            return e
        except Exception as e:
            raise e

        return json_result

async def start_up_asyncio_client() -> None:
    logger.info("start up asyncio client")
    SingletonAiohttp.get_aiohttp_client()

async def close_asyncio_client() -> None:
    logger.info("close asyncio client")
    await SingletonAiohttp.close_aiohttp_client()