import sys
sys.path.insert(0,"..")
from opcua import Client
from opcua import ua
import numpy as np
import cv2
from picamera import PiCamera
from picamera.array import PiRGBArray
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

# ---------------------------------------------- START CLIENT ---------------------------------------------- #
client = Client("opc.tcp://147.229.132.230:4840")
client.connect()

# --------------------------------------- INITIALIZATION PARAMETERS ---------------------------------------- #
# manual buttons {Boolean}
get_info  = client.get_node("ns=6;s=::Program:rpi_cam.get_info")
done      = client.get_node("ns=6;s=::Program:rpi_cam.done")
# type {Float, String}
part_state = client.get_node("ns=6;s=::Program:rpi_cam.part_state")
part_type  = client.get_node("ns=6;s=::Program:rpi_cam.part_type")
color_str  = client.get_node("ns=6;s=::Program:rpi_cam.color")
# center position of the part {Float}
center_pos_diamater_a = client.get_node("ns=6;s=::Program:rpi_cam.center_position.diameter.a")
center_pos_diamater_b = client.get_node("ns=6;s=::Program:rpi_cam.center_position.diameter.b")
center_pos_diamater_rot = client.get_node("ns=6;s=::Program:rpi_cam.center_position.diameter.rotation")
center_pos_x = client.get_node("ns=6;s=::Program:rpi_cam.center_position.x")
center_pos_y = client.get_node("ns=6;s=::Program:rpi_cam.center_position.y")
# center position of the part {Float}
gripper_pos_x = client.get_node("ns=6;s=::Program:rpi_cam.gripper_position.x")
gripper_pos_y = client.get_node("ns=6;s=::Program:rpi_cam.gripper_position.y")
# position of the holes {Float}
# hole -> Center
cam_hole_center_d = client.get_node("ns=6;s=::Program:rpi_cam.hole_center.diameter")
cam_hole_center_x = client.get_node("ns=6;s=::Program:rpi_cam.hole_center.x")
cam_hole_center_y = client.get_node("ns=6;s=::Program:rpi_cam.hole_center.y")
# hole -> Right Top
cam_hole_rt_d = client.get_node("ns=6;s=::Program:rpi_cam.hole_right_top.diameter")
cam_hole_rt_x = client.get_node("ns=6;s=::Program:rpi_cam.hole_right_top.x")
cam_hole_rt_y = client.get_node("ns=6;s=::Program:rpi_cam.hole_right_top.y")
# hole -> Right Bottom
cam_hole_rb_d = client.get_node("ns=6;s=::Program:rpi_cam.hole_right_bottom.diameter")
cam_hole_rb_x = client.get_node("ns=6;s=::Program:rpi_cam.hole_right_bottom.x")
cam_hole_rb_y = client.get_node("ns=6;s=::Program:rpi_cam.hole_right_bottom.y")
# hole -> Left Top
cam_hole_lt_d = client.get_node("ns=6;s=::Program:rpi_cam.hole_left_top.diameter")
cam_hole_lt_x = client.get_node("ns=6;s=::Program:rpi_cam.hole_left_top.x")
cam_hole_lt_y = client.get_node("ns=6;s=::Program:rpi_cam.hole_left_top.y")
# hole -> Left Bottom
cam_hole_lb_d = client.get_node("ns=6;s=::Program:rpi_cam.hole_left_bottom.diameter")
cam_hole_lb_x = client.get_node("ns=6;s=::Program:rpi_cam.hole_left_bottom.x")
cam_hole_lb_y = client.get_node("ns=6;s=::Program:rpi_cam.hole_left_bottom.y")

# --------------------------------------- FUNCTION ---------------------------------------- #
def set_param(done_f, part_state_f, part_type_f, color, center_pos_da, center_pos_db, center_pos_dr, cent_pos_x, cent_pos_y, gripp_pos_x, gripp_pos_y, holer_c_d, holer_c_x, holer_c_y, holer_rt_d, holer_rt_x, holer_rt_y, holer_rb_d, holer_rb_x, holer_rb_y, holer_lt_d, holer_lt_x, holer_lt_y, holer_lb_d, holer_lb_x, holer_lb_y):
    # done
    dv_done = ua.DataValue(ua.Variant(done_f, ua.VariantType.Boolean))
    done.set_value(dv_done)

    # part state
    dv_part_state = ua.DataValue(ua.Variant(part_state_f, ua.VariantType.Float))
    part_state.set_value(dv_part_state)

    # part type
    dv_part_type  = ua.DataValue(ua.Variant(part_type_f, ua.VariantType.Float))
    part_type.set_value(dv_part_type)

    # color
    dv_color_str  = ua.DataValue(ua.Variant(color, ua.VariantType.String))
    color_str.set_value(dv_color_str)

    # center position of the part {Float}
    # diameter a
    dv_center_pos_diamater_a = ua.DataValue(ua.Variant(center_pos_da, ua.VariantType.Float))
    center_pos_diamater_a.set_value(dv_center_pos_diamater_a)

    # diameter b
    dv_center_pos_diamater_b = ua.DataValue(ua.Variant(center_pos_db, ua.VariantType.Float))
    center_pos_diamater_b.set_value(dv_center_pos_diamater_b)

    # rotation
    dv_center_pos_diamater_rot = ua.DataValue(ua.Variant(center_pos_dr, ua.VariantType.Float))
    center_pos_diamater_rot.set_value(dv_center_pos_diamater_rot)

    # postion x
    dv_center_pos_x = ua.DataValue(ua.Variant(cent_pos_x, ua.VariantType.Float))
    center_pos_x.set_value(dv_center_pos_x)

    # position y
    dv_center_pos_y = ua.DataValue(ua.Variant(cent_pos_y, ua.VariantType.Float))
    center_pos_y.set_value(dv_center_pos_y)

    # gripper position of the part {Float}
    # position x
    dv_gripper_pos_x = ua.DataValue(ua.Variant(gripp_pos_x, ua.VariantType.Float))
    gripper_pos_x.set_value(dv_gripper_pos_x)

    # position y
    dv_gripper_pos_y = ua.DataValue(ua.Variant(gripp_pos_y, ua.VariantType.Float))
    gripper_pos_y.set_value(dv_gripper_pos_y)

    # hole -> Center
    # diameter
    dv_cam_hole_center_d = ua.DataValue(ua.Variant(holer_c_d, ua.VariantType.Float))
    cam_hole_center_d.set_value(dv_cam_hole_center_d)

    # position x
    dv_cam_hole_center_x = ua.DataValue(ua.Variant(holer_c_x, ua.VariantType.Float))
    cam_hole_center_x.set_value(dv_cam_hole_center_x)

    # position y
    dv_cam_hole_center_y = ua.DataValue(ua.Variant(holer_c_y, ua.VariantType.Float))
    cam_hole_center_y.set_value(dv_cam_hole_center_y)

    # hole -> Right Top
    # diameter
    dv_cam_hole_rt_d = ua.DataValue(ua.Variant(holer_rt_d, ua.VariantType.Float))
    cam_hole_rt_d.set_value(dv_cam_hole_rt_d)

    # position x
    dv_cam_hole_rt_x = ua.DataValue(ua.Variant(holer_rt_x, ua.VariantType.Float))
    cam_hole_rt_x.set_value(dv_cam_hole_rt_x)

    # position y
    dv_cam_hole_rt_y = ua.DataValue(ua.Variant(holer_rt_y, ua.VariantType.Float))
    cam_hole_rt_y.set_value(dv_cam_hole_rt_y)

    # hole -> Right Bottom
    # diameter
    dv_cam_hole_rb_d = ua.DataValue(ua.Variant(holer_rb_d, ua.VariantType.Float))
    cam_hole_rb_d.set_value(dv_cam_hole_rb_d)

    # position x
    dv_cam_hole_rb_x = ua.DataValue(ua.Variant(holer_rb_x, ua.VariantType.Float))
    cam_hole_rb_x.set_value(dv_cam_hole_rb_x)

    # position y
    dv_cam_hole_rb_y = ua.DataValue(ua.Variant(holer_rb_y, ua.VariantType.Float))
    cam_hole_rb_y.set_value(dv_cam_hole_rb_y)

    # hole -> Left Top
    # diameter
    dv_cam_hole_lt_d = ua.DataValue(ua.Variant(holer_lt_d, ua.VariantType.Float))
    cam_hole_lt_d.set_value(dv_cam_hole_lt_d)

    # position x
    dv_cam_hole_lt_x = ua.DataValue(ua.Variant(holer_lt_x, ua.VariantType.Float))
    cam_hole_lt_x.set_value(dv_cam_hole_lt_x)

    # position y
    dv_cam_hole_lt_y = ua.DataValue(ua.Variant(holer_lt_y, ua.VariantType.Float))
    cam_hole_lt_y.set_value(dv_cam_hole_lt_y)

    # hole -> Left Bottom
    # diameter
    dv_cam_hole_lb_d = ua.DataValue(ua.Variant(holer_lb_d, ua.VariantType.Float))
    cam_hole_lb_d.set_value(dv_cam_hole_lb_d)

    # position x
    dv_cam_hole_lb_x = ua.DataValue(ua.Variant(holer_lb_x, ua.VariantType.Float))
    cam_hole_lb_x.set_value(dv_cam_hole_lb_x)

    # position y
    dv_cam_hole_lb_y = ua.DataValue(ua.Variant(holer_lb_y, ua.VariantType.Float))
    cam_hole_lb_y.set_value(dv_cam_hole_lb_y)


# capturing a picture from PiCamera

# initialize the camera and grab a reference to the raw camera capture
camera = PiCamera()
rawCapture = PiRGBArray(camera)

# allow the camera to warmup
time.sleep(0.1)

# grab an image from the camera
camera.capture(rawCapture, format="bgr")
image = rawCapture.array


# filters to be applied
image_hsv = cv2.cvtColor(image, cv2.COLOR_BGR2HSV)
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
width=1.96850394


# color detection

# green color
low_orange = np.array([10, 127, 144])
high_orange = np.array([255, 255, 243])
green_mask = cv2.inRange(image_hsv, low_orange, high_orange)

# orange color
low_orange = np.array([0, 100, 180])
high_orange = np.array([14, 255, 255])
orange_mask = cv2.inRange(image_hsv, low_orange, high_orange)

# white color
low_white = np.array([10, 88, 144])
high_white = np.array([20, 255, 255])
white_mask = cv2.inRange(image_hsv, low_white, high_white)

# black color
low_black = np.array([0, 0, 0])
high_black = np.array([255, 226, 70])
black_mask = cv2.inRange(image_hsv, low_black, high_black)

# gray color
low_gray = np.array([0, 45, 106])
high_gray = np.array([120, 255, 255])
gray_mask = cv2.inRange(image_hsv, low_gray, high_gray)

 
# mask creation
mask = [orange_mask, gray_mask, white_mask, green_mask, black_mask]
c_t  = ['orange', 'gray', 'white','green', 'black']


result_c = 0
for i in range(0, len(mask)):
	contours=cv2.findContours(mask[i],cv2.RETR_TREE,cv2.CHAIN_APPROX_SIMPLE)[-2]

    # looping over the contours
	for pic, contour in enumerate(contours):
		area = cv2.contourArea(contour)
		if area>2500 and area<8500:
			color_rpi = c_t[i]
			part_type_rpi = i+1
			print(color_rpi) 
			x,y,w,h = cv2.boundingRect(contour)	
			cv2.rectangle(image,(x,y),(x+w,y+h),(255,0,0),2)
			result_c = 1

	if result_c == 1:
		break
		


# detection of faultness

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 lines 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("wrong location of a hole")
	            	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)


# values for vizualization 

# done_f, part_state_f, part_type_f, color, center_pos_da, center_pos_db, center_pos_dr, cent_pos_x, cent_pos_y, gripp_pos_x, gripp_pos_y, holer_c_d, holer_c_x, holer_c_y, holer_rt_d, holer_rt_x, holer_rt_y, holer_rb_d, holer_rb_x, holer_rb_y, holer_lt_d, holer_lt_x, holer_lt_y, holer_lb_d, holer_lb_x, holer_lb_y
set_param(True, wrong, part_type_rpi, color_rpi, 50.12, 50.05, angle, li2[0][0], li2[0][1], gX, gY, 15.09, li2[1][0], li2[1][1], 5.07, li2[2][0], li2[2][1], 4.98, li2[3][0], li2[3][1], 5.1, li2[4][0], li2[4][1], 4.92, li2[5][0], li2[5][1])

cv2.imshow("image",image)
#cv2.imwrite("image-%s.jpg" % time.strftime("%H-%M-%S"),image)


cv2.waitKey(0)
cv2.destroyAllWindows()