#!/usr/bin/env python3

import rospy
import cv2
import numpy as np
from pyzbar.pyzbar import decode
from sensor_msgs.msg import Image, LaserScan
from geometry_msgs.msg import Twist
from cv_bridge import CvBridge, CvBridgeError
import random

TARGET_DISTANCE = 0.2  # Meters
ANGULAR_SPEED = 0.5
LINEAR_SPEED = 0.2

class QRCodeFollower:
    def __init__(self):
        rospy.init_node('qr_code_follower', anonymous=False)
        self.bridge = CvBridge()
        self.cmd_vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
        self.image_sub = rospy.Subscriber("/camera/image", Image, self.image_callback)
        self.laser_sub = rospy.Subscriber("/scan", LaserScan, self.laser_callback)
        
        self.qr_code_found = False
        self.distance_to_owner = None

    def rotate_360(self):
        twist = Twist()
        twist.angular.z = ANGULAR_SPEED
        duration = 7.85 / ANGULAR_SPEED  # Time to complete a full rotation
        start_time = rospy.Time.now().to_sec()
        while rospy.Time.now().to_sec() - start_time < duration:
            self.cmd_vel_pub.publish(twist)
        twist.angular.z = 0
        self.cmd_vel_pub.publish(twist)

    def move_random_direction(self):
        twist = Twist()
        twist.linear.x = LINEAR_SPEED
        duration = random.uniform(1, 3)  # Move for a random duration
        start_time = rospy.Time.now().to_sec()
        while rospy.Time.now().to_sec() - start_time < duration:
            self.cmd_vel_pub.publish(twist)
        twist.linear.x = 0
        self.cmd_vel_pub.publish(twist)

    def detect_qr_code(self, image):
        decoded_objects = decode(image)
        for obj in decoded_objects:
            if obj.data.decode() == "Owner":
                self.qr_code_found = True
                
                points = obj.polygon
                if len(points) > 4: 
                    hull = cv2.convexHull(np.array([point for point in points], dtype=np.float32))
                    points = hull
                n = len(points)
                for j in range(n):
                    cv2.line(image, points[j], points[(j+1) % n], (255,0,0), 3)
                
                cv2.imshow("QR Code Detection", image)
                cv2.waitKey(1)
                return True
        return False

    def image_callback(self, msg):
        try:
            cv_image = self.bridge.imgmsg_to_cv2(msg, "bgr8")
        except CvBridgeError as e:
            rospy.logerr(e)
            return
        
        if not self.detect_qr_code(cv_image) and not self.qr_code_found:
            self.rotate_360()
            self.move_random_direction()
            self.rotate_360()

    def laser_callback(self, msg):
        self.distance_to_owner = min(msg.ranges)

    def follow_qr_code(self):
        twist = Twist()
        if self.qr_code_found and self.distance_to_owner > TARGET_DISTANCE:
            twist.linear.x = LINEAR_SPEED
            rospy.loginfo(f"Distance to owner: {self.distance_to_owner:.2f} meters")
        else:
            twist.linear.x = 0
            rospy.loginfo("Reached the target distance to owner.")
        self.cmd_vel_pub.publish(twist)

    def run(self):
        rate = rospy.Rate(10)  # 10 Hz
        while not rospy.is_shutdown():
            if self.qr_code_found:
                self.follow_qr_code()
            rate.sleep()

    def shutdown(self):
        rospy.loginfo("Stopping QRCodeFollower...")
        self.cmd_vel_pub.publish(Twist())
        cv2.destroyAllWindows()

if __name__ == '__main__':
    qr_follower = QRCodeFollower()
    rospy.on_shutdown(qr_follower.shutdown)
    try:
        qr_follower.run()
    except rospy.ROSInterruptException:
        pass