#! /usr/bin/python

import rospy
from sensor_msgs.msg import Image
from cv_bridge import CvBridge
import numpy as np
import dynamic_reconfigure.server

from vqimg.msg import component_centers, component_center
from find_targets import find_targets, normalize_coordinates
from detect_targets.cfg import DetectTargetsConfig


class Publisher:
    def __init__(self):
        self.threshold_blue = (140, 255)
        self.threshold_red = (0, 120)
        self.threshold_green = (0, 190)
        self.binary_view = False
        self.draw_targets = False
        self.centers = component_centers()
        self.pub = rospy.Publisher('targets', component_centers, queue_size=10)
        self.img_pub = rospy.Publisher('img_targets', Image, queue_size=0)
        rospy.Subscriber("/bebop/image_raw", Image, self.on_picture)
        self.bridge = CvBridge()
        self.config_srv = dynamic_reconfigure.server.Server(DetectTargetsConfig, self.on_reconf)

    def on_reconf(self, config, level):
        self.threshold_red = (config['red_min'], config['red_max'])
        self.threshold_blue = (config['blue_min'], config['blue_max'])
        self.threshold_green = (config['green_min'], config['green_max'])
        self.binary_view = config['binary']
        self.draw_targets = config['targets']
        return config

    def on_picture(self, msg):
        img = np.array(np.asarray(self.bridge.imgmsg_to_cv2(msg)), copy=True)
        try:
            if not self.binary_view:
                H, L, R = find_targets(
                    img,
                    threshold_blue=self.threshold_blue,
                    threshold_red=self.threshold_red,
                    threshold_green=self.threshold_green
                )
            else:
                H, L, R, points = find_targets(
                    img,
                    threshold_blue=self.threshold_blue,
                    threshold_red=self.threshold_red,
                    threshold_green=self.threshold_green,
                    return_binary=True
                )

        except ValueError:
            return
        pub_H = normalize_coordinates(H, msg.width, msg.height)
        pub_L = normalize_coordinates(L, msg.width, msg.height)
        pub_R = normalize_coordinates(R, msg.width, msg.height)
        self.centers.data = [
            component_center(x=pub_H[0], y=pub_H[1], nb_vertex=1, label=1),
            component_center(x=pub_L[0], y=pub_L[1], nb_vertex=1, label=2),
            component_center(x=pub_R[0], y=pub_R[1], nb_vertex=1, label=3),
        ]

        self.pub.publish(self.centers)
        if self.binary_view:
            img[:,:,1] = points * 255
            img[:,:,0] = np.zeros((len(img), len(img[0])))
            img[:,:,2] = np.zeros((len(img), len(img[0])))
            if self.draw_targets:
                for i in range(-20, 20):
                    for j in range(-20, 20):
                        try:
                            img[int(H[1])-i, int(H[0])-j] = [0, 255, 0]
                            img[int(L[1])-i, int(L[0])-j] = [255, 0, 0]
                            img[int(R[1])-i, int(R[0])-j] = [0, 0, 255]
                        except Exception:
                            pass
            self.img_pub.publish(self.bridge.cv2_to_imgmsg(img, "rgb8"))


if __name__ == '__main__':
    rospy.init_node('target_publisher')

    publisher = Publisher()
    rospy.spin()