# from picamera import PiCamera
# from picamera.array import PiRGBArray
from time import sleep
import time
import numpy as np
import cv2 as cv
import imutils
from imutils import contours


# from matplotlib import pyplot as plt

# Cropping function
def crop_image(img, x, x_length, y, y_length):
    if x >= 0 and x_length >= 0 and y >= 0 and y_length >= 0:
        crop_img = img[x: x + x_length, y: y + y_length]
    else:
        crop_img = img
    return crop_img


# Function for calculation
def MM_distance(px):
    MM = 0.0211 * px + 0.1806
    return MM

# Parametr definition
scale = 1
scale2 = 2
scale3 = 4

photo = 10

# Photo capture
if photo == 1:
    start = time.time()
    camera = PiCamera()
    camera.rotation = 270
    camera.resolution = (4056, 3040)  # (3904,2928) # (4056,3040)#(1600,1200)
    camera.capture('Img.png')
    print("Trigger time: " + str(time.time() - start))

# Timer for image processing
start = time.time()

## Image loading
Image = cv.imread('Img.png')
height, width = Image.shape[:2]
print('image dimension', height, width)
Img = cv.resize(Image, (int(width / scale), int(height / scale)), interpolation=cv.INTER_CUBIC)

# VUT logo Localization
hsv_image = cv.cvtColor(Img, cv.COLOR_BGR2HSV)
low_red = np.array([165, 70, 50])
high_red = np.array([180, 255, 255])
cv.imwrite('hsv.png', hsv_image)
mask = cv.inRange(hsv_image, low_red, high_red)
ret, thresh = cv.threshold(mask, 200, 255, 0)
Contours, _ = cv.findContours(thresh, cv.RETR_EXTERNAL, cv.CHAIN_APPROX_SIMPLE)


# Rectangle drawing
for c in Contours:
    x, y, w, h = cv.boundingRect(c)
    if w > 500 and h > 500:
        cv.rectangle(Img, (x, y), (x + w, y + h), (0, 0, 0), 2)
        center_x = x + w / 2
        center_y = y + h / 2
        cv.circle(Img, (int(center_x), int(center_y)), 5, (0, 0, 0), 2)

        print(x, y, w, h)

cv.circle(Img, (int(center_x), int(center_y)), 5, (0, 0, 0), 2)
Img2 = cv.resize(Img, (int(width / scale3), int(height / scale3)), interpolation=cv.INTER_CUBIC)
cv.imshow('Finded Logo',Img2)

# Scaling of center coordinates
Rx_center = int(center_x * scale)
Ry_center = int(center_y * scale)
print('center of logo is ', Ry_center, Rx_center)
cv.circle(Image, (Rx_center, Ry_center), 5, (0, 0, 0,), 4)

# Cropping of image
croped_img = crop_image(Image, Ry_center - 1300, 500, Rx_center + 300, 1200)
cv.imwrite("croped image clear.png", croped_img)

# Finding contours
gray = cv.cvtColor(croped_img, cv.COLOR_BGR2GRAY)

edged = cv.Canny(gray, 40, 85)
edged = cv.dilate(edged, None, iterations=2)
edged = cv.erode(edged, None, iterations=2)

height_crop, width_crop = croped_img.shape[:2]
edged2 = cv.resize(edged, (int(width_crop / scale2), int(height_crop / scale2)), interpolation=cv.INTER_CUBIC)

cnts = cv.findContours(edged.copy(), cv.RETR_EXTERNAL, cv.CHAIN_APPROX_SIMPLE)
cnts = imutils.grab_contours(cnts)
(sort_cnts, _) = contours.sort_contours(cnts)

i = 0

rects = np.zeros((10, 4))
y_min = 10000
for c in sort_cnts:
    x, y, w, h = cv.boundingRect(c)
    if y < y_min:
        y_min = y
h_max = 350
print('hmax', h_max, ',  ymin ', y_min)

for c in sort_cnts:
    x, y, w, h = cv.boundingRect(c)
    if w >= 120 and h >= 120:
        h = h_max
        y = y_min
        cv.rectangle(croped_img, (x, y), (x + w, y + h), (0, 0, 0), 2)
        rects[i] = ([x, y, w, h])
        print('rect ', i, ' - ', x, y, w, h)
        i = i + 1
print(rects)

# Point operation
j = 1
if rects[0, 0] < 10:
    for k in rects:
        if not rects[j, 0] <= 150:
            midle0 = np.array([0, 0])
            midle0[0] = int(rects[0, 0] + rects[0, 2])
            midle0[1] = int(rects[0, 1] + (rects[0, 3] / 2))

            midle1 = np.array([0, 0])
            midle1[0] = int(rects[j, 0])
            midle1[1] = int(rects[j, 1] + (rects[j, 3] / 2))

            high0 = np.array([0, 0])
            high0[0] = rects[j, 0]
            high0[1] = rects[j, 1]

            low0 = np.array([0, 0])
            low0[0] = rects[j, 0]
            low0[1] = rects[j, 1] + rects[j, 3]

            croped_img = cv.line(croped_img, (midle0[0], midle0[1]),
                                 (midle1[0], midle1[1]), (255, 0, 0), 1)

            A1 = int(midle0[0])
            A2 = int(midle0[1])
            cv.circle(croped_img, (A1, A2), 5, (255, 0, 0), 1)

            B1 = int(high0[0])
            B2 = int(high0[1])
            cv.circle(croped_img, (B1, B2), 10, (255, 0, 0), 1)

            C1 = int(low0[0])
            C2 = int(low0[1])
            cv.circle(croped_img, (C1, C2), 15, (255, 0, 0), 1)
            break
        else:
            j = j + 1

print('low point ', low0)
print('midle point ', midle0)
print('high point ', high0)

# Trigonometry calculation
a = np.sqrt((B1 - C1) ** 2 + (B2 - C2) ** 2)
print('a ', a)
b = np.sqrt((A1 - C1) ** 2 + (A2 - C2) ** 2)
print('b ', b)
c = np.sqrt((A1 - B1) ** 2 + (A2 - B2) ** 2)
print('c ', c)

# Main calculation
v = np.sqrt(c ** 2 - ((a ** 2 + c ** 2 - b ** 2) / (2 * a)) ** 2)
print('distance is ', v, ' px')
distance = MM_distance(v)
print('distance is ', distance, ' mm')
distance = round(distance, 5)

output_text = (str(distance) + ' mm')
cv.putText(croped_img, output_text, (300, 200), cv.FORMATTER_FMT_DEFAULT, 2, (240, 10, 10))
cv.imshow('Final distance', croped_img)

# timer stop
print("Image processing time: " + str(time.time() - start))

cv.waitKey(0)





