import numpy as np
import cv2
import time
from scipy.spatial import distance as dist
import argparse
import math


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

def thirdpoint(ptC, ptD):
    return ((ptC[0] + ptD[0]) * 0.7, (ptC[1] + ptD[1]) * 0.5)

def get_angle(p1, p2):
    return math.atan2(p2[1] - p1[1], p2[0] - p1[0]) * 180/math.pi

def order_points(pts):
    # sort the points based on their x-coordinates
    xSorted = pts[np.argsort(pts[:, 0]), :]

    # grab the left-most and right-most points from the sorted
    # x-roodinate points
    leftMost = xSorted[:2, :]
    rightMost = xSorted[2:, :]

    # now, sort the left-most coordinates according to their
    # y-coordinates so we can grab the top-left and bottom-left
    # points, respectively
    leftMost = leftMost[np.argsort(leftMost[:, 1]), :]
    (tl, bl) = leftMost

    # now that we have the top-left coordinate, use it as an
    # anchor to calculate the Euclidean distance between the
    # top-left and right-most points; by the Pythagorean
    # theorem, the point with the largest distance will be
    # our bottom-right point
    D = dist.cdist(tl[np.newaxis], rightMost, "euclidean")[0]
    (br, tr) = rightMost[np.argsort(D)[::-1], :]

    # return the coordinates in top-left, top-right,
    # bottom-right, and bottom-left order
    return np.array([tl, tr, br, bl], dtype="float32")

def is_near_in_list(li, cC):
    cX, cY, area = cC
    for x, y, z in li:
        if abs(x-cX) < 30 and abs(y-cY) < 30 and abs(z-area) < 1000:
            return True
    return False



# input
image = cv2.imread('image-12-13-04.jpg')
image_hsv = cv2.cvtColor(image, cv2.COLOR_BGR2HSV)

# filters to be applied
blurred = cv2.GaussianBlur(image_hsv, (5, 5), 0)
thresh = cv2.threshold(blurred, 250, 255, cv2.THRESH_TRUNC)[1]
edged = cv2.Canny(thresh, 30, 70)

#measured width of a part (inches)
width=1.96850394


# detection of faultness

# finding countours in an image
contours = cv2.findContours(edged, cv2.RETR_LIST, cv2.CHAIN_APPROX_NONE)[-2]
contours = sorted(contours, key=cv2.contourArea, reverse=True)
li=[]




for c in contours:
    if cv2.contourArea(c) < 1:
        continue

    # contour approximation
    p      = cv2.arcLength(c, True)
    approx = cv2.approxPolyDP(c, 0.01 * p, True)

    # finding moments of contours
    M = cv2.moments(c)
    if M['m00'] == 0.0:
                continue

    cX = int(M['m10']/M['m00'])
    cY = int(M['m01']/M['m00'])

    area=cv2.contourArea(c)
    cC=(cX,cY,area)


    # storing unique contours in a list
    if not is_near_in_list(li, cC):
      li.append(cC)

    # drawing contours on an image
    cv2.circle(image, (cX,cY), 2, (0,0,255), 2)
    cv2.drawContours(image, [c], 0, (0, 0, 255), 2)


# removing the parameter area from list
li2=[]
for f in li:
	cX2=f[0]
	cY2=f[1]
	cC2=(cX2,cY2)
	li2.append(cC2)
	
print(li2)



# checking for a REPAIR part
if len(li2) == 5:
	print("REPAIR")
	wrong=3

# checking for a BAD part
else: 
	
	# taking the biggest contour by area	
	cnts = cv2.findContours(edged, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_NONE)[-2]
	cnts = sorted(cnts, key=cv2.contourArea, reverse=True)

	for c in cnts:
	    box = cv2.minAreaRect(c)
	    # for the use on RPi modify this line as follows 
	    #box=cv2.cv.BoxPoints(box)
	    box = cv2.boxPoints(box)
	    box = np.array(box, dtype="int")


	   
	    # definition of points of a bounding rectangle
	    box = order_points(box)
	    (tl, tr, br, bl) = box
	    (tlblX, tlblY) = midpoint(tl, bl)
	    (trbrX, trbrY) = midpoint(tr, br)
	 

	    # draw line between the midpoints - reference line
	    cv2.line(image, (int(tlblX), int(tlblY)), (int(trbrX), int(trbrY)),
	        (255, 0, 255), 2)

	    
	    # compute the Euclidean distance between the midpoints of a bounding box
	    dB = dist.euclidean((tlblX, tlblY), (trbrX, trbrY))


	    # if the pixels per metric has not been initialized, then
	    # compute it as the ratio of pixels to supplied metric
	    # (in this case, inches)
	    pixelsPerMetric = dB / width
	    if pixelsPerMetric == 0.0:
	    	continue

	    dimB = dB / pixelsPerMetric
	    # inch to mm
	    dimB = dimB*25.4


	    # show a reference length
	    cv2.putText(image, "{:.1f}mm".format(dimB),
	        (int(trbrX - 260), int(trbrY - 22)), cv2.FONT_HERSHEY_SIMPLEX,
	        0.65, (255, 255, 255), 2)



	    for i in range(1, len(li2)):
	     
	        # draw circles corresponding to the current points and
	        # connect them with a line
	        cv2.circle(image, (li2[0]), 5, (255,0,0), -1)
	        cv2.circle(image, (li2[i]), 5, (255,0,0), -1)
	        
	    	        # compute the distance between middle and each point
	        D = dist.euclidean((li2[0]), (li2[i])) / pixelsPerMetric
	        # inch to mm
	        D = D*25.4
	        

	        # claiming of faultness
	        if i ==1: 
	            if D <= 3:
	            	wrong=1
	                

	            else:
	            	print("BAD")
	            	wrong=2
	            	break
	       

# definition of a gripper point
if len(li2)==5:
	(rX, rY) = midpoint(li2[3], li2[4])
	(gX, gY) =(rX, rY)
else:
	(gX, gY) = midpoint(li2[2], li2[4])
	
cv2.circle(image, (int(gX), int(gY)), 5, (255,0,255), -1)
print(gX,gY)

# rotation
angle=get_angle((li2[0]), (gX,gY))
print(angle)



cv2.imshow("edged",edged)
cv2.imshow("image",image)


cv2.waitKey(0)
cv2.destroyAllWindows()
