#!/usr/bin/env python

#from Baxter import *

import sys
import cv2
import os
#from sensor_msgs.msg import Image
#from cv_bridge import CvBridge, CvBridgeError

from shapedetector import ShapeDetector
import imutils
"""
from geometry_msgs.msg import (
    PoseStamped,
    Pose,
    Point,
    Quaternion,
)
"""

img_format = ".jpg"


#baxter = Baxter_Robot("right")

#"Corner\nx=0.472849478978 y=0.295796754786 z=-0.173375298769 x=0.342246552482 y=0.93797464198 z=0.044132681924 w=-0.0335137986772\n"
"""
pose = Pose()

pose.position.x = 0.4
pose.position.y = 0.
pose.position.z = 0.6
pose.orientation.x = 1.
pose.orientation.y = 0.
pose.orientation.z = 0.
pose.orientation.w = -1.

joint_angles = baxter.ik_request(pose)

#baxter.move_to_joint_angles(joint_angles)


#joint_angles['right_w2'] -= 2.5

#baxter.move_to_joint_angles(joint_angles)

#baxter.close_grippers()

"""

image_topic = "/cameras/" + "right_hand_camera" + "/image"
path=os.path.expanduser('~/FER/proba/')
img_name = "green2"
if not os.path.exists(path):
	os.makedirs(path)

img_path = path + img_name + img_format
"""
bridge = CvBridge()

msg = rospy.wait_for_message(image_topic, Image)
try:
	cv2_img = bridge.imgmsg_to_cv2(msg, "bgr8")
except CvBridgeError, e:
	print(e)

cv2.imwrite(img_path, cv2_img)
"""
#take_screenshot(index="_01")


image = cv2.imread(img_path)
resized = imutils.resize(image, width=300)
ratio = image.shape[0] / float(resized.shape[0])

# convert the resized image to grayscale, blur it slightly,
# and threshold it
gray = cv2.cvtColor(resized, cv2.COLOR_BGR2GRAY)
blurred = cv2.GaussianBlur(gray, (5, 5), 0)
thresh = cv2.threshold(blurred, 60, 255, cv2.THRESH_BINARY)[1]

# find contours in the thresholded image and initialize the
# shape detector
cnts = cv2.findContours(thresh.copy(), cv2.RETR_EXTERNAL,
	cv2.CHAIN_APPROX_SIMPLE)
cnts = imutils.grab_contours(cnts)
sd = ShapeDetector()
print(cnts)

for c in cnts:
	# compute the center of the contour, then detect the name of the
	# shape using only the contour
	M = cv2.moments(c)
	cX = int((M["m10"] / M["m00"]) * ratio)
	cY = int((M["m01"] / M["m00"]) * ratio)
	shape = sd.detect(c)

	# multiply the contour (x, y)-coordinates by the resize ratio,
	# then draw the contours and the name of the shape on the image
	c = c.astype("float")
	c *= ratio
	c = c.astype("int")
	cv2.drawContours(image, [c], -1, (0, 255, 0), 2)
	cv2.putText(image, shape, (cX, cY), cv2.FONT_HERSHEY_SIMPLEX,
		0.5, (255, 255, 255), 2)

	# show the output image
	cv2.imshow("Image", image)
	cv2.waitKey(500)