import cv2
import numpy as np
from bsp import *

# Подключаемся к камере
cam = cv2.VideoCapture(0)

# Мотор
m = motor()
speed = 10

while True:
    _, frame = cam.read()
    # Перевод изображения в HSV
    hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
    # Немного размываем изображение
    hsv = cv2.blur(hsv,(5,5))

    # Задаем параметр из прошлого примера
    mask = cv2.inRange(hsv, (0,190,129),(21,255,255))

    # С помощью специального алгоритма ищем контур обьекта
    (contours, hierarchy) = cv2.findContours(mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
    # Отрисовывем найденый контур чтобы его было видно на экране
    cv2.drawContours(frame, contours, -1, (255,0,0), 3, cv2.LINE_AA, hierarchy, 1 )

    max_radius = 0
    center = (0,0)

    # Находим контур с максимальным радиусом
    x = 0
    y = 0

    for contour in contours:
        (x,y),radius = cv2.minEnclosingCircle(contour)
        if max_radius < int(radius):
            max_radius = int(radius)
            center = (int(x),int(y))

    # Описываем максимальный контур кругом        
    frame = cv2.circle(frame,center,max_radius,(0,255,0),2)

    S = 3.1415 * max_radius * max_radius
    # Выводим на эран площадь круга
    cv2.putText(frame, "S: " + str(S) + " X: " + str(x) + " Y: " + str(y), (30, 30),cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 10, 10), 2)

    # # Выводим сообщение если круг слишком близко или далеко
    # if S > 100:
    #     if S > 80000:
    #         cv2.putText(frame, "backward", (30, 60),cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 10, 10), 2)
    #         m.backward(speed)
    #     elif S < 40000:
    #         cv2.putText(frame, "forward", (30, 60),cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 10, 10), 2)
    #         m.forward(speed)
    #     else:
    #         m.stop()

    

    delta = 100
    max_x = 640
    if (x < max_x/2 - delta):
        m.left(speed)
        time.sleep(0.3)
        m.stop()
        time.sleep(0.1)
    elif (x > max_x/2 + delta):
        m.right(speed)
        time.sleep(0.3)
        m.stop()
        time.sleep(0.1)
    else:
        if S > 100 and S < 40000:
            m.forward(speed)
        else:
            m.stop()

        # # конечный автомат:
        # # 0 - центрирование относительно метки
        # # 1 - Подбезжаем к метке
        # # 2 - ожидаем выхода
        # state = 0 
        # delta = 100
        # max_x = 640

        # if state == 0:
        #     if (x < max_x/2 - delta):
        #         m.left(speed)
        #     elif (x > max_x/2 + delta):
        #         m.right(speed)
        #     else:
        #         state = 1

        # elif state == 1:
        #     if (S < 7000):
        #         m.forward(speed)
        #     else:
        #         state = 2

        #     if (x < max_x/2 - delta or x > max_x/2 - delta):
        #         state = 0
        # else:
        #     if (x < max_x/2 - delta or x > max_x/2 - delta):
        #         state = 0
        #     elif (S < 7000):
        #         state = 1
        #     else:
        #         time.sleep(1)

        # cv2.putText(frame, "State: " + str(state), (30, 60),cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 10, 10), 2)


    # Промежуточные изображения
    #cv2.imshow("Image with opening", mask)
    #cv2.imshow("Image with closing", hsv) 
    
    # выводим то что получилось вместе с текстом и контуром
    cv2.imshow("Image", frame)

    # m.stop()
    # time.sleep(1)
 
    k = cv2.waitKey(2)
    if k == 27:
        cv2.destroyAllWindows()
        break