# coding=utf-8
import rospy
import cv2
import numpy as np
import matplotlib.pyplot as plt
import random
import math
from skimage import measure
from cv_bridge import CvBridge, CvBridgeError
from sensor_msgs.msg import Image
from std_msgs.msg import String


import datetime
from geometry_msgs.msg import Twist


class Follower:

    def __init__(self):
        self.bridge = CvBridge()

        self.image_sub = rospy.Subscriber('/d415/color/image_raw', Image, self.image_callback)
        self.cmd_vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1)

        self.twist = Twist()
        self.err = 0
        self.turn_cmd = Twist()

        self.signal = False
        self.begin = 0

    def image_callback(self, msg):
        # To get the image from the camera and convert it to binary image by using opencv
        image = self.bridge.imgmsg_to_cv2(msg, desired_encoding='bgr8')

        hsv = cv2.cvtColor(image, cv2.COLOR_BGR2HSV)

        # Define the threshold of red and green
        lower_red = np.array([170, 43, 40])
        upper_red = np.array([180, 255, 255])


        # Segment red line and green line from hsv image and convert it to  binary image
        mask_red = cv2.inRange(hsv, lower_red, upper_red)

        mask = mask_red
        mask = cv2.erode(mask, (3,3))
        h, w, d = image.shape
        search_top = int(h / 2)
        search_bot = search_top + 100
        mask[0:search_top, :] = 0
        #mask[search_bot:h, :] = 0
        mask, contours, hierarchy = cv2.findContours(mask, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)
        areas = []


        for cont in contours:
            area = cv2.contourArea(cont)
            areas.append(area)

        # To find the optimal index of contour
        if len(areas) == 0:
            return False

        sort = np.argsort(areas)
        index = sort[-1]
        print len(areas)
        cv2.drawContours(image, contours, index, (0, 0, 255), 3)


        if cv2.contourArea(contours[index]) >= 100:
            # Computing the centroid of the contours of binary image
            M = cv2.moments(contours[index])
            self.signal = True
            cx = int(M['m10'] / M['m00'])
            cy = int(M['m01'] / M['m00'])
            cv2.circle(image, (cx, cy), 20, (255, 0, 0), -1)

            # P-controller
            self.err = cx - w/2
            self.twist.linear.x = 0.4
            self.twist.angular.z = -float(self.err) / 45
            self.cmd_vel_pub.publish(self.twist)

            self.begin = datetime.datetime.now().second
        # else:
        #     # Define actions when the point is not be found
        #     # 5 HZ
        #     r = rospy.Rate(5)
        #     # let's go forward at 0.1 m/s
        #     # by default angular.z is 0 so setting this isn't required
        #     move_cmd = Twist()
        #     move_cmd.linear.x = 0.1

        #     if self.signal is True:
        #         if self.err < 0:
        #             self.turn_cmd.angular.z = math.radians(30)
        #         elif self.err > 0:
        #             self.turn_cmd.angular.z = math.radians(-30)

        #         self.cmd_vel_pub.publish(self.turn_cmd)
        #         now = datetime.datetime.now().second

        #         # Go forward about 0.2m, then stop
        #         if math.fabs(now - self.begin) > 2:
        #             for x in range(0, 10):
        #                 self.cmd_vel_pub.publish(move_cmd)
        #                 r.sleep()
        #             self.signal = False

        cv2.imshow('window1', image)
        cv2.waitKey(1)
        #cv2.destroyAllWindows()


if __name__ == "__main__":
    rospy.init_node('follow')
    follow = Follower()
    rospy.spin()







    # lower_red = np.array([170, 43, 40])
    # upper_red = np.array([180, 255, 255])

    # for i in range(1,100):
    #     dir = "./color/" + str(i) +".jpg"
    #     image = cv2.imread(dir)
    #     hsv = cv2.cvtColor(image, cv2.COLOR_BGR2HSV)
    #     mask = cv2.inRange(hsv, lower_red, upper_red)

    #     mask = cv2.erode(mask, (3,3))
    #     h, w, d = image.shape
    #     search_top = 2 * h / 5
    #     search_bot = search_top + 100
    #     mask[0:search_top, :] = 0
    #     #mask[search_bot:h, :] = 0
    #     mask, contours, hierarchy = cv2.findContours(mask, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)
    #     areas = []


    #     for cont in contours:
    #         area = cv2.contourArea(cont)
    #         areas.append(area)

    #     # To find the optimal index of contour
    #     if len(areas) == 0:
    #         continue

    #     sort = np.argsort(areas)
    #     index = sort[-1]
    #     print len(areas)
    #     cv2.drawContours(image, contours, index, (0, 0, 255), 3)

    #     if cv2.contourArea(contours[index]) <= 100:
    #         continue

    #     M = cv2.moments(contours[index])
    #     cx = int(M['m10'] / M['m00'])
    #     cy = int(M['m01'] / M['m00'])
    #     cv2.circle(image, (cx, cy), 20, (255, 0, 0), -1)

   
    #     cv2.imshow('image', image)
    #     cv2.waitKey(0)
    #     cv2.destroyAllWindows()