# import the necessary packages
#from imutils import perspective
from imutils import contours
import numpy as np
import argparse
import imutils
import cv2
import math
import time 

itemw = 0
itemh = 0


def midpoint(ptA, ptB):
	return ((ptA[0] + ptB[0]) * 0.5, (ptA[1] + ptB[1]) * 0.5)

def sizeVexScrew(iteml):
	# Screw Sizing code
	# subtract screw head size to find thread length
	shead = 0.1
	iteml -= shead
	#print("Thread Length: " + str(iteml))
	iteml *= 8
	iteml = round(iteml)
	iteml /= 8
	return iteml

def sizeStandoff(iteml):
	# Standoff Sizing code
	iteml *= 2
	iteml = round(iteml)
	iteml /= 2
	return iteml


def larger(a, b):
	if a >= b:
		return a
	else: 
		return b

def smaller(a, b):
	if a < b:
		return a
	else: 
		return b

def near(a, b, close):
	if abs(a-b) < close:
		return True
	return False
# construct the argument parse and parse the arguments
ap = argparse.ArgumentParser()
ap.add_argument("-i", "--image", required=True,
	help="path to the input image")
ap.add_argument("-w", "--width", type=float, required=True,
	help="width of the left-most object in the image (in inches)")
ap.add_argument("-n", "--number", type=int, required=False,
	help="object # to measure (from left to right)")
ap.add_argument("-s", "--show", action="store_true",
	help="show on the screen")
args = vars(ap.parse_args())
args2 = ap.parse_args()
selected = 2
if type(args["number"]) == type(selected):
	selected = args["number"]

# load the image, convert it to grayscale, and blur it slightly
image = cv2.imread(args["image"])
#image = cv2.resize(image, (int(image.shape[1]*1), int(image.shape[0]*1)))
image = cv2.resize(image, (1000, int(image.shape[0]/image.shape[1] * 1000)), interpolation = cv2.INTER_NEAREST)

if args2.show:
	cv2.imshow("Item Sorter", image)
	cv2.waitKey(0)
gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)
gray = cv2.GaussianBlur(gray, (7, 7), 0)


# perform edge detection, then perform a dilation + erosion to
# close gaps in between object edges
edged = cv2.Canny(gray, 50, 100)
edged = cv2.dilate(edged, None, iterations=1)
#edged = cv2.erode(edged, None, iterations=1)

if args2.show:
	cv2.imshow("Item Sorter", edged)
	cv2.waitKey(0)
# find contours in the edge map
cnts = cv2.findContours(edged.copy(), cv2.RETR_EXTERNAL,
	cv2.CHAIN_APPROX_SIMPLE)
cnts = imutils.grab_contours(cnts)

# sort the contours from left-to-right and initialize the
# 'pixels per metric' calibration variable
(cnts, _) = contours.sort_contours(cnts)
pixelsPerMetric = None
num = 0





# Calibration loop
for c in cnts:
	# if the contour is not sufficiently large, ignore it
	if cv2.contourArea(c) < 100:
		continue
	# compute the rotated bounding box of the contour
	orig = image.copy()
	box = cv2.minAreaRect(c)
	box = cv2.cv.BoxPoints(box) if imutils.is_cv2() else cv2.boxPoints(box)
	box = np.array(box, dtype="int")
	#box = perspective.order_points(box)
	(tl, tr, br, bl) = box
	(tltrX, tltrY) = midpoint(tl, tr)
	(blbrX, blbrY) = midpoint(bl, br)
	(tlblX, tlblY) = midpoint(tl, bl)
	(trbrX, trbrY) = midpoint(tr, br)
	dA = np.linalg.norm(np.array((tltrX, tltrY, 0)) - np.array((blbrX, blbrY, 0)))
	dB = np.linalg.norm(np.array((tlblX, tlblY, 0)) - np.array((trbrX, trbrY, 0)))
	area_box = dA * dB
	(x,y),radius = cv2.minEnclosingCircle(c)
	area_contour = cv2.contourArea(c)
	area_circle = math.pi * pow(radius, 2)
	boxiness = area_contour / area_box
	circleness = area_contour / area_circle
	circular = False
	rectangular = False
	if boxiness > circleness:
		rectangular = True
		cv2.drawContours(orig, [box.astype("int")], -1, (0, 255, 0), 2)
	else:
		circular = True
		cv2.circle(orig,(int(x),int(y)),int(radius),(0,255,0),2)
		mask = np.zeros(gray.shape,np.uint8)
		cv2.drawContours(mask,[c],0,255,-1)
		#pixelpoints = np.transpose(np.nonzero(mask))
		hsv = cv2.cvtColor(orig, cv2.COLOR_BGR2HSV)
		mean_val = cv2.mean(hsv,mask = mask)
		print(str(mean_val[0]))
		#print(", " + str(mean_val[0]/mean_val[2]))
		#print(", " + str(mean_val[2]/mean_val[1]))
	if pixelsPerMetric is None and circular is True and near(mean_val[0], 16, 4.5):
		# and near(mean_val[0], 63, 40) is True and near(mean_val[1], 108, 40) is True and near(mean_val[2], 104, 40) is True:
		pixelsPerMetric = smaller(dA, dB) / args["width"]
		


orig = image.copy()
# loop over the contours individually
for c in cnts:
	
	num += 1
	# if the contour is not sufficiently large, ignore it
	if cv2.contourArea(c) < 100 or pixelsPerMetric is None:
		continue
	# compute the rotated bounding box of the contour
	

	box = cv2.minAreaRect(c)
	box = cv2.cv.BoxPoints(box) if imutils.is_cv2() else cv2.boxPoints(box)
	box = np.array(box, dtype="int")

	# order the points in the contour such that they appear
	# in top-left, top-right, bottom-right, and bottom-left
	# order, then draw the outline of the rotated bounding
	# box
	#box = perspective.order_points(box)

	# loop over the original points and draw them
	#for (x, y) in box:
		#cv2.circle(orig, (int(x), int(y)), 5, (0, 0, 255), -1)

	# unpack the ordered bounding box, then compute the midpoint
	# between the top-left and top-right coordinates, followed by
	# the midpoint between bottom-left and bottom-right coordinates
	(tl, tr, br, bl) = box
	(tltrX, tltrY) = midpoint(tl, tr)
	(blbrX, blbrY) = midpoint(bl, br)

	# compute the midpoint between the top-left and top-right points,
	# followed by the midpoint between the top-righ and bottom-right
	(tlblX, tlblY) = midpoint(tl, bl)
	(trbrX, trbrY) = midpoint(tr, br)

	# draw the midpoints on the image
	#cv2.circle(orig, (int(tltrX), int(tltrY)), 5, (255, 0, 0), -1)
	#cv2.circle(orig, (int(blbrX), int(blbrY)), 5, (255, 0, 0), -1)
	#cv2.circle(orig, (int(tlblX), int(tlblY)), 5, (255, 0, 0), -1)
	#cv2.circle(orig, (int(trbrX), int(trbrY)), 5, (255, 0, 0), -1)

	# draw lines between the midpoints
    # compute the Euclidean distance between the midpoints
	dA = np.linalg.norm(np.array((tltrX, tltrY, 0)) - np.array((blbrX, blbrY, 0)))
	dB = np.linalg.norm(np.array((tlblX, tlblY, 0)) - np.array((trbrX, trbrY, 0)))


	dimA = dA / pixelsPerMetric
	dimB = dB / pixelsPerMetric
	
	if num == selected or args2.show:
		area_box = dA * dB
		(x,y),radius = cv2.minEnclosingCircle(c)
		area_contour = cv2.contourArea(c)
		area_circle = math.pi * pow(radius, 2)
		boxiness = area_contour / area_box
		circleness = area_contour / area_circle
		circular = False
		rectangular = False
		if boxiness > circleness:
			rectangular = True
			cv2.drawContours(orig, [box.astype("int")], -1, (0, 255, 0), 2)
		else:
			circular = True
			cv2.circle(orig,(int(x),int(y)),int(radius),(0,255,0),2)
		
		
		objtype = "Unknown"
		itemw = larger(dimA, dimB)
		itemwr = itemw
		itemwr *= 8
		itemwr = round(itemwr)
		itemwr /= 8

		itemh = smaller(dimA, dimB)
		itemhr = itemh
		itemhr *= 16
		itemhr = round(itemhr)
		itemhr /= 16
		if circular and itemwr == 0.75:
			objtype = "Penny"
			iteml = 0
		else:
			epsilon = 3#0.02*cv2.arcLength(c,True)
			#print(str(epsilon))
			approx = cv2.approxPolyDP(c,epsilon,True)
			hull = cv2.convexHull(approx, returnPoints=False)
			hull2 = cv2.convexHull(c)
			defects = cv2.convexityDefects(c,hull)
			#print(str(defects.size) + " match")
			cv2.drawContours(orig, (hull2), -1, (0, 0, 255), 8)
			cv2.drawContours(orig, (approx), -1, (255, 0, 0), 6)
			convexness = area_contour / cv2.contourArea(hull2)
			#print(str(convexness) + " % fill")
			#if not cv2.isContourConvex(approx):
			#if cv2.matchShapes(hull, c, 1, 0.0) > 1:
			if defects.size > 5 and (convexness < 0.9 or boxiness < 0.75):
				objtype = "Screw"
				iteml = larger(dimA, dimB)
				#print("Screw Length (RAW): " + str(iteml))
				iteml = sizeVexScrew(radius * 2 / pixelsPerMetric)
				#print("Rounded Length: " + str(iteml))
			else:
				if itemhr == 0.3125:
					objtype = "Standoff"
					iteml = sizeStandoff(itemw)
					
				if itemhr == 0.1875:
					objtype = "Axle"
					iteml = (radius * 2 / pixelsPerMetric + itemw) / 2

				
		#print(str(iteml))
		# draw the object sizes on the image
		if args2.show:
			#cv2.putText(orig, "{:.5f}in".format(itemhr),
			#	(int(trbrX + 20), int(trbrY)), cv2.FONT_HERSHEY_SIMPLEX,
			#	0.65, (255, 255, 255), 2)
			cv2.putText(orig, str(objtype),
				(int(trbrX + 20), int(trbrY)), cv2.FONT_HERSHEY_SIMPLEX,
				0.65, (255, 255, 255), 2)
			if objtype == "Unknown":
				cv2.putText(orig, "{:.2f}in".format(itemw) + " x {:.2f}in".format(itemh), # print axle length
					(int(trbrX + 20), int(trbrY + 20)), cv2.FONT_HERSHEY_SIMPLEX,
					0.65, (255, 255, 255), 2)
			if objtype == "Screw":
				cv2.putText(orig, str(iteml) + "in thread", # print screw length
					(int(trbrX + 20), int(trbrY + 20)), cv2.FONT_HERSHEY_SIMPLEX,
					0.65, (255, 255, 255), 2)
			if objtype == "Standoff":
				cv2.putText(orig, str(iteml) + "in", # print standoff length
					(int(trbrX + 20), int(trbrY + 20)), cv2.FONT_HERSHEY_SIMPLEX,
					0.65, (255, 255, 255), 2)
			if objtype == "Axle":
				cv2.putText(orig, "{:.2f}in".format(iteml), # print axle length
					(int(trbrX + 20), int(trbrY + 20)), cv2.FONT_HERSHEY_SIMPLEX,
					0.65, (255, 255, 255), 2)
 
	# show the output image
cv2.imshow("Item Sorter", orig)
cv2.waitKey(0)