#!python

import logging

logger = logging.getLogger(f"rosout.{__name__}")

import sys
import asyncio
import time

from officebots import Robot

officebots_logger = logging.getLogger("officebots")

from math import cos, sin, pi, floor, sqrt

import cv2
import numpy as np
from cv_bridge import CvBridge

import rospy
from geometry_msgs.msg import Twist
from nav_msgs.msg import Odometry
from sensor_msgs.msg import LaserScan, Image

import tf


argv = rospy.myargv(argv=sys.argv)

if len(argv) < 2:
    print("Usage: %s <robot name>" % argv[0])
    sys.exit(1)

robot_name = argv[1]


def restore_logging():
    handler = logging.StreamHandler()
    handler.setFormatter(
        logging.Formatter("[%(levelname)s] %(asctime)s - %(name)s: %(message)s")
    )
    officebots_logger.addHandler(handler)
    officebots_logger.setLevel(logging.DEBUG)


class RosOfficeBots(Robot):

    base_frame = "base_link"

    def __init__(self):
        super().__init__()
        rospy.init_node("officebots")
        restore_logging()
        rospy.Subscriber("cmd_vel", Twist, self.on_cmd_vel)

        self.cv_bridge = CvBridge()
        self.br = tf.TransformBroadcaster()

        self.odom_pub = rospy.Publisher("odom", Odometry, queue_size=1)
        self.odom_msg = Odometry()
        self.odom_msg.header.frame_id = "odom"
        self.odom_msg.child_frame_id = self.base_frame
        self.odom_msg.pose.pose.position.z = 0.0
        self.odom_msg.pose.pose.orientation.x = 0.0
        self.odom_msg.pose.pose.orientation.y = 0.0

        # laser scan parameters

        nb_rays = 50
        self.angle_increment = pi / (nb_rays - 1)
        self.angle_min = -pi / 2
        self.angle_max = pi / 2
        self.range_min = 0.0  # m
        self.range_max = 20.0  # m

        self.scan_pub = rospy.Publisher("scan", LaserScan, queue_size=1)
        self.scan_msg = LaserScan()
        self.scan_msg.header.frame_id = self.base_frame
        self.scan_msg.angle_min = self.angle_min
        self.scan_msg.angle_max = self.angle_max
        self.scan_msg.angle_increment = self.angle_increment
        self.scan_msg.time_increment = 0.0
        self.scan_msg.range_min = self.range_min
        self.scan_msg.range_max = self.range_max

        self.image_pub = rospy.Publisher("camera/rgb/image_raw", Image, queue_size=1)

        self.last = time.time()

    async def run(self):
        res = await self.execute([robot_name, "create"])
        if res[0] != self.OK:
            logger.warning(res[1])
        else:
            logger.info(f"Created the robot {robot_name}")

        logger.info("ROS Officebots bridge ready")

        # simply keeps python from exiting until this node is stopped
        while not rospy.is_shutdown():
            status, jpg_data = await self.execute([robot_name, "export-camera"])
            # await asyncio.sleep(0.1)
            array = np.asarray(bytearray(jpg_data), dtype=np.uint8)
            image = cv2.imdecode(array, cv2.IMREAD_COLOR)
            image_msg = self.cv_bridge.cv2_to_imgmsg(image, encoding="passthrough")
            rospy.logwarn("publishing image")
            self.image_pub.publish(image_msg)

        self.stop()
        logger.info("Bye")

    async def on_robot_update(self, data):

        now = time.time()
        dt = now - self.last
        self.last = now

        x, y, theta, v, w = data["odom"]

        qx, qy, qz, qw = tf.transformations.quaternion_from_euler(0, 0, theta)

        self.odom_msg.header.stamp = rospy.Time.now()
        self.odom_msg.pose.pose.position.x = x
        self.odom_msg.pose.pose.position.y = y

        self.odom_msg.pose.pose.orientation.z = qz
        self.odom_msg.pose.pose.orientation.w = qw

        self.odom_msg.twist.twist.linear.x = v
        self.odom_msg.twist.twist.angular.z = w

        self.odom_pub.publish(self.odom_msg)

        self.br.sendTransform(
            (x, y, 0), (qx, qy, qz, qw), rospy.Time.now(), self.base_frame, "odom"
        )

        self.scan_msg.scan_time = dt
        self.scan_msg.ranges = data["laserscan"]
        self.scan_msg.header.stamp = rospy.Time.now()

        self.scan_pub.publish(self.scan_msg)

    def on_cmd_vel(self, twist):
        x = twist.linear.x
        w = twist.angular.z

        self._event_loop.create_task(self.execute(["WallE", "cmd-vel", [x, w]]))


if __name__ == "__main__":

    RosOfficeBots().start()
