import cv2
from cv_bridge import CvBridge, CvBridgeError
import rospy
from sensor_msgs.msg import Image

"""
Starter code for an image processing node. Subscribes to the downward camera
and publishes a processed cv image (i.e. colorized, overlayed with a detected line)
This node is not meant to be a full node that you will use in your final line tracking
solution, but it will teach you key concepts that you will need for line following
"""

class ImageToCV:
    def __init__(self):
        # A subscriber to the topic '/aero_downward_camera/image'. self.image_sub_cb is called when a message is recieved
        self.camera_sub = rospy.Subscriber("/aero_downward_camera/image", Image, self.image_cb)

        # A publisher which will publish a parametrization of the detected line to the topic '/image_to_cv/processed'
        self.image_pub = rospy.Publisher("/image_to_cv/processed", Image, queue_size=1)
        self.bridge = CvBridge()

    def image_cb(self, msg):
        """
        Callback function which is called when a new message of type Image is recieved by self.camera_sub.

            Args:
                - msg = ROS Image message in 8UC1 format

            Returns: NA
        """
        try:
            cv_image = self.bridge.imgmsg_to_cv2(msg, "8UC1")
            self.process(cv_image)
        except CvBridgeError as e:
            print(e)

    def process(self, img):
        """
        Takes in an img param and finds the line in it, if it exists

            Args:
                - img : OpenCV Image to process

            Publishes:
                - annotated image to the /image_to_cv/processed topic
        """
        """TODO-START: FILL IN CODE HERE"""
        # Get a colored copy of the image. This will be used solely to provide an annotated version
        # of the image for debuging purposes (for you to see in rqt_image_view). See slides for help here

        # Threshold the image to isolate the line and use it for your linear regression algorithm
        # (tip: check size to ensure you are detecting the LED line or a simple blip on the screen)

        # "Draw" a line if you have found one on the image

        # Publish your newly annotated, color image
        # NOTE: If you do not detect a line, or if you have decided to ignore detected lines of very small
        #       size, be sure to still publish the image. Also be sure to have some way to handle the camera
        #       seeing multiple "lines" at one time


        """TODO-END"""

if __name__ == "__main__":
    rospy.init_node("image_to_cv")
    a = ImageToCV()

    rospy.spin()