#!/usr/bin/env python
import cv2 as cv # opencv library name to cv
import rospy #ros python

from sensor_msgs.msg import Image
from geometry_msgs.msg import Point

rec_point=Point()

rospy.init_node("furo_find")
coordinates_pub = rospy.Publisher("target_coordinate", Point, queue_size=10) #publish x,y coordinate to the node
## Trained XML file for detecting face with its path
face_cascade = cv.CascadeClassifier('/home/dasl/opencv/opencv-4.2.0/data/haarcascades/haarcascade_frontalface_alt.xml')
# Trained XML file for detecting eyes
eye_cascade = cv.CascadeClassifier('/home/dasl/opencv/opencv-4.2.0/data/haarcascades/haarcascade_eye.xml')

# Capture frames from a camera
cap = cv.VideoCapture(0,cv.CAP_V4L)
while True:
	ret, img = cap.read()
	# Convert to gray scale of each frames
	gray = cv.cvtColor(img, cv.COLOR_BGR2GRAY)

	# Detects faces of different sizes in the input image
	faces = face_cascade.detectMultiScale(gray, 1.3, 5)

	for (x,y,w,h) in faces:
		# To draw a rectangle in a face
		cv.rectangle(img,(x,y),(x+w,y+h),(255,255,0),2)
		roi_gray = gray[y:y+h, x:x+w]
		roi_color = img[y:y+h, x:x+w]
		target_x=(w/2.0)+x
		target_y=(h/2.0)+y
		
		target_direction_x = target_x/640
		target_direction_y = target_y/480
		
		target_direction_x -= 0.5
		target_direction_y -= 0.5

		target_direction_x *= 2.0
		target_direction_y *= 2.0
		
		rec_point.x = target_direction_x
		rec_point.y = target_direction_y
	
		# Detects eyes of different sizes in the input image
	#	eyes = eye_cascade.detectMultiScale(roi_gray)

		#To draw a rectangle in eyes
		#for (ex,ey,ew,eh) in eyes:
		#	cv.rectangle(roi_color,(ex,ey),(ex+ew,ey+eh),(0,127,255),2)

	##Use the below comment if you need to display an image in a window
	screen_res=2560,1440
	window_width=2560
	window_height=1440
	cv.namedWindow('img',cv.WINDOW_NORMAL)
	resized=cv.resize(img,screen_res)
	cv.resizeWindow('img',window_width, window_height)
	flipped =cv.flip(resized,1)
	cv.imshow('img', flipped)
	h,w,c=img.shape
	#print(w)
	#print('width:',img.width)
	#print('height:',img.height)
	

	coordinates_pub.publish(rec_point)
	rospy.loginfo(rec_point)
	rate = rospy.Rate(200)
	#rate.sleep()

	# Wait for Esc key to stop
	if cv.waitKey(5) ==27:
	    break