import math
import imageio
import cv2 as cv
import numpy as np
import transformer

def fix_rotation(img):
    img_copy = img.copy()
    img = cv.cvtColor(img, cv.COLOR_BGR2GRAY)
    rows, cols = img.shape
    img = cv.adaptiveThreshold(img, 255, cv.ADAPTIVE_THRESH_MEAN_C, cv.THRESH_BINARY_INV, 15, 9)

    kernel = cv.getStructuringElement(cv.MORPH_ELLIPSE, (3, 3))
    img = cv.morphologyEx(img, cv.MORPH_OPEN, kernel)

    img = cv.medianBlur(img, 3)

    contours, hierarchy = cv.findContours(img, cv.RETR_LIST, cv.CHAIN_APPROX_NONE)
    roi = max(contours, key=cv.contourArea)
    
    x, y, w, h = cv.boundingRect(roi)
    corners = [[x, y], [x + w, y], [x, y + h], [x + w, y + h]]
    src = np.float32(corners)
    # src = np.reshape(src, (len(src), 1, 2))

    # perimeter = cv.arcLength(src, True)
    # corners = cv.approxPolyDP(src, perimeter // 10, True)
    # corners = np.vstack(corners)
    dst = np.float32([[0, 0], [cols, 0], [0, rows], [cols, rows]])

    matrix = cv.getPerspectiveTransform(src, dst)
    rotated_img = cv.warpPerspective(img_copy, matrix, (cols, rows))
    cv.imshow('', rotated_img)

D1 = 105
D2 = 175
D3 = 275

if __name__ == "__main__":

    cap = cv.VideoCapture('samples/delta.mp4')
    if not cap.isOpened():
        raise IOError("Video was not opened!")

    mse = 0
    count = 0

    reader = imageio.get_reader('samples/delta.mp4')
    fps = reader.get_meta_data()['fps']
    writer = imageio.get_writer('samples/result.mp4', fps=fps)

    while True:
        res, frame = cap.read()
        if not res:
            break

        mean_error = 0
        holes_count = 0

        img = frame.copy()
        cv.imshow('dfa', img)
        frame = cv.cvtColor(frame, cv.COLOR_BGR2GRAY)
        frame_copy = frame.copy()
        # frame = cv.adaptiveThreshold(frame, 255, cv.ADAPTIVE_THRESH_MEAN_C, cv.THRESH_BINARY_INV, 15, 9)

        # kernel = cv.getStructuringElement(cv.MORPH_ELLIPSE, (3, 3))
        # frame  = cv.morphologyEx(frame, cv.MORPH_OPEN, kernel)

        # frame = cv.medianBlur(frame, 3)
        # contours, hierarchy = cv.findContours(frame, cv.RETR_LIST, cv.CHAIN_APPROX_NONE)
        # roi = max(contours, key=cv.contourArea)
        # x, y, w, h = cv.boundingRect(roi)
        x, y, w, h = 115, 0, 445, 360
        img = img[y: y+h, x: x+w]
        img = transformer.rotate_along_axis(img, theta=40)
        frame_copy = frame_copy[y: y+h, x: x+w]
        frame_copy = transformer.rotate_along_axis(frame_copy, theta=40)
        # cv.imshow('', frame_copy)
        # cv.rectangle(frame_copy, (x, y), (x + w, y + h), (0, 255, 0), 2)
        # cv.drawContours(frame_copy, roi, -1, (0, 0, 255), 2)

        # res, mask = cv.threshold(frame_copy, 0, 255, cv.THRESH_BINARY)
        # frame_copy = cv.bitwise_and(frame_copy, frame_copy, mask=mask)
        # corners = cv.goodFeaturesToTrack(frame_copy, 1000, 0.0001, 1)
        # corners = list(sorted(corners, key=lambda x: x[0][1]))
        # print(corners[-1], corners[-2])
        # print()

        # corners = np.array([[38, 293], [407, 293]])
        # for item in corners:
        #     # x, y = map(int, item.ravel())
        #     x, y = item
        #     cv.circle(img, (x, y), 5, (0, 0, 255), -1)

        src = np.float32([[0, 0], [w, 0], [38, 293], [407, 293]])
        dst = np.float32([[0, 0], [w, 0], [30, h], [w - 30, h]])
        matrix = cv.getPerspectiveTransform(src, dst)
        img = cv.warpPerspective(img, matrix, (w, h))
        cv.imshow('', img)

        img_copy = img.copy()
        img = cv.cvtColor(img, cv.COLOR_BGR2GRAY)
        img = cv.adaptiveThreshold(img, 255, cv.ADAPTIVE_THRESH_MEAN_C, cv.THRESH_BINARY_INV, 15, 9)

        kernel = cv.getStructuringElement(cv.MORPH_ELLIPSE, (3, 3))
        img = cv.morphologyEx(img, cv.MORPH_OPEN, kernel)
        img = cv.medianBlur(img, 3)

        origin = (w // 2 + 4, h // 2 + 2)
        o1, o2 = origin
        r = w // 2 + 1

        ORIGIN = (0, 0)
        R = 300 # mm

        contours, hierarchy = cv.findContours(img, cv.RETR_LIST, cv.CHAIN_APPROX_NONE)
        contours = list(filter(lambda x: 50 < cv.contourArea(x) < 175, contours))
        
        factor = 0.1
        smooth_contours = []
        for i in range(len(contours)):
            epsilon = factor * cv.arcLength(contours[i], True)
            approx = cv.approxPolyDP(contours[i], epsilon, True)
            
            x, y, width, height = cv.boundingRect(approx)
            area = width*height
            if len(approx) == 4 and 75 < area < 200:
                smooth_contours.append(contours[i])

                center, radius = cv.minEnclosingCircle(approx)
                radius = int(radius)
                center = tuple(map(int, center))

                x, y = center
                X = ((x - o1) * R) / r
                Y = ((y - o2) * R) / r

                X, Y = round(X, 2), round(Y, 2)

                cv.circle(img_copy, center, radius, (0, 255, 0), 2)
                cv.putText(img_copy, str((X, Y)), center, cv.FONT_HERSHEY_SIMPLEX, 0.3, (255, 0, 255, 255), 1, cv.LINE_AA)

                e1, e2, e3 = map(lambda d: abs(math.hypot(X, Y) - d), [D1, D2, D3])
                error = min(e1, e2, e3)
                if error < 10:
                    mean_error += error ** 2
                    holes_count += 1

        cv.circle(img_copy, origin, 4, (0, 0, 255), -1)
        # cv.line(img_copy, origin, (origin[0], origin[1]), (255, 0, 255), 2)

        mean_error /= holes_count
        mse += mean_error
        count += 1
        cv.imshow("Final", img_copy)
        writer.append_data(img_copy)
        # cv.imshow("Chg", img)
        if cv.waitKey(30) == 27:
            break

    print("E:", mse / count, "N:", count)
    writer.close()
    cap.release()
    cv.destroyAllWindows()
[((9, 10, 9, 45), 'cv2.cvtColor', 'cv.cvtColor', ({(9, 22, 9, 25): 'img', (9, 27, 9, 44): 'cv.COLOR_BGR2GRAY'}, {}), '(img, cv.COLOR_BGR2GRAY)', True, 'import cv2 as cv\n'), ((11, 10, 11, 96), 'cv2.adaptiveThreshold', 'cv.adaptiveThreshold', ({(11, 31, 11, 34): 'img', (11, 36, 11, 39): '255', (11, 41, 11, 66): 'cv.ADAPTIVE_THRESH_MEAN_C', (11, 68, 11, 88): 'cv.THRESH_BINARY_INV', (11, 90, 11, 92): '15', (11, 94, 11, 95): '9'}, {}), '(img, 255, cv.ADAPTIVE_THRESH_MEAN_C, cv.\n    THRESH_BINARY_INV, 15, 9)', True, 'import cv2 as cv\n'), ((13, 13, 13, 63), 'cv2.getStructuringElement', 'cv.getStructuringElement', ({(13, 38, 13, 54): 'cv.MORPH_ELLIPSE', (13, 56, 13, 62): '(3, 3)'}, {}), '(cv.MORPH_ELLIPSE, (3, 3))', True, 'import cv2 as cv\n'), ((14, 10, 14, 53), 'cv2.morphologyEx', 'cv.morphologyEx', ({(14, 26, 14, 29): 'img', (14, 31, 14, 44): 'cv.MORPH_OPEN', (14, 46, 14, 52): 'kernel'}, {}), '(img, cv.MORPH_OPEN, kernel)', True, 'import cv2 as cv\n'), ((16, 10, 16, 31), 'cv2.medianBlur', 'cv.medianBlur', ({(16, 24, 16, 27): 'img', (16, 29, 16, 30): '3'}, {}), '(img, 3)', True, 'import cv2 as cv\n'), ((18, 26, 18, 82), 'cv2.findContours', 'cv.findContours', ({(18, 42, 18, 45): 'img', (18, 47, 18, 59): 'cv.RETR_LIST', (18, 61, 18, 81): 'cv.CHAIN_APPROX_NONE'}, {}), '(img, cv.RETR_LIST, cv.CHAIN_APPROX_NONE)', True, 'import cv2 as cv\n'), ((21, 17, 21, 37), 'cv2.boundingRect', 'cv.boundingRect', ({(21, 33, 21, 36): 'roi'}, {}), '(roi)', True, 'import cv2 as cv\n'), ((23, 10, 23, 29), 'numpy.float32', 'np.float32', ({(23, 21, 23, 28): 'corners'}, {}), '(corners)', True, 'import numpy as np\n'), ((29, 10, 29, 66), 'numpy.float32', 'np.float32', ({(29, 21, 29, 65): '[[0, 0], [cols, 0], [0, rows], [cols, rows]]'}, {}), '([[0, 0], [cols, 0], [0, rows], [cols, rows]])', True, 'import numpy as np\n'), ((31, 13, 31, 49), 'cv2.getPerspectiveTransform', 'cv.getPerspectiveTransform', ({(31, 40, 31, 43): 'src', (31, 45, 31, 48): 'dst'}, {}), '(src, dst)', True, 'import cv2 as cv\n'), ((32, 18, 32, 68), 'cv2.warpPerspective', 'cv.warpPerspective', ({(32, 37, 32, 45): 'img_copy', (32, 47, 32, 53): 'matrix', (32, 55, 32, 67): '(cols, rows)'}, {}), '(img_copy, matrix, (cols, rows))', True, 'import cv2 as cv\n'), ((33, 4, 33, 30), 'cv2.imshow', 'cv.imshow', ({(33, 14, 33, 16): '""""""', (33, 18, 33, 29): 'rotated_img'}, {}), "('', rotated_img)", True, 'import cv2 as cv\n'), ((41, 10, 41, 46), 'cv2.VideoCapture', 'cv.VideoCapture', ({(41, 26, 41, 45): '"""samples/delta.mp4"""'}, {}), "('samples/delta.mp4')", True, 'import cv2 as cv\n'), ((48, 13, 48, 52), 'imageio.get_reader', 'imageio.get_reader', ({(48, 32, 48, 51): '"""samples/delta.mp4"""'}, {}), "('samples/delta.mp4')", False, 'import imageio\n'), ((50, 13, 50, 62), 'imageio.get_writer', 'imageio.get_writer', (), '', False, 'import imageio\n'), ((164, 4, 164, 26), 'cv2.destroyAllWindows', 'cv.destroyAllWindows', ({}, {}), '()', True, 'import cv2 as cv\n'), ((61, 8, 61, 29), 'cv2.imshow', 'cv.imshow', ({(61, 18, 61, 23): '"""dfa"""', (61, 25, 61, 28): 'img'}, {}), "('dfa', img)", True, 'import cv2 as cv\n'), ((62, 16, 62, 53), 'cv2.cvtColor', 'cv.cvtColor', ({(62, 28, 62, 33): 'frame', (62, 35, 62, 52): 'cv.COLOR_BGR2GRAY'}, {}), '(frame, cv.COLOR_BGR2GRAY)', True, 'import cv2 as cv\n'), ((75, 14, 75, 58), 'transformer.rotate_along_axis', 'transformer.rotate_along_axis', (), '', False, 'import transformer\n'), ((77, 21, 77, 72), 'transformer.rotate_along_axis', 'transformer.rotate_along_axis', (), '', False, 'import transformer\n'), ((95, 14, 95, 65), 'numpy.float32', 'np.float32', ({(95, 25, 95, 64): '[[0, 0], [w, 0], [38, 293], [407, 293]]'}, {}), '([[0, 0], [w, 0], [38, 293], [407, 293]])', True, 'import numpy as np\n'), ((96, 14, 96, 64), 'numpy.float32', 'np.float32', ({(96, 25, 96, 63): '[[0, 0], [w, 0], [30, h], [w - 30, h]]'}, {}), '([[0, 0], [w, 0], [30, h], [w - 30, h]])', True, 'import numpy as np\n'), ((97, 17, 97, 53), 'cv2.getPerspectiveTransform', 'cv.getPerspectiveTransform', ({(97, 44, 97, 47): 'src', (97, 49, 97, 52): 'dst'}, {}), '(src, dst)', True, 'import cv2 as cv\n'), ((98, 14, 98, 53), 'cv2.warpPerspective', 'cv.warpPerspective', ({(98, 33, 98, 36): 'img', (98, 38, 98, 44): 'matrix', (98, 46, 98, 52): '(w, h)'}, {}), '(img, matrix, (w, h))', True, 'import cv2 as cv\n'), ((99, 8, 99, 26), 'cv2.imshow', 'cv.imshow', ({(99, 18, 99, 20): '""""""', (99, 22, 99, 25): 'img'}, {}), "('', img)", True, 'import cv2 as cv\n'), ((102, 14, 102, 49), 'cv2.cvtColor', 'cv.cvtColor', ({(102, 26, 102, 29): 'img', (102, 31, 102, 48): 'cv.COLOR_BGR2GRAY'}, {}), '(img, cv.COLOR_BGR2GRAY)', True, 'import cv2 as cv\n'), ((103, 14, 103, 100), 'cv2.adaptiveThreshold', 'cv.adaptiveThreshold', ({(103, 35, 103, 38): 'img', (103, 40, 103, 43): '255', (103, 45, 103, 70): 'cv.ADAPTIVE_THRESH_MEAN_C', (103, 72, 103, 92): 'cv.THRESH_BINARY_INV', (103, 94, 103, 96): '15', (103, 98, 103, 99): '9'}, {}), '(img, 255, cv.ADAPTIVE_THRESH_MEAN_C, cv.\n    THRESH_BINARY_INV, 15, 9)', True, 'import cv2 as cv\n'), ((105, 17, 105, 67), 'cv2.getStructuringElement', 'cv.getStructuringElement', ({(105, 42, 105, 58): 'cv.MORPH_ELLIPSE', (105, 60, 105, 66): '(3, 3)'}, {}), '(cv.MORPH_ELLIPSE, (3, 3))', True, 'import cv2 as cv\n'), ((106, 14, 106, 57), 'cv2.morphologyEx', 'cv.morphologyEx', ({(106, 30, 106, 33): 'img', (106, 35, 106, 48): 'cv.MORPH_OPEN', (106, 50, 106, 56): 'kernel'}, {}), '(img, cv.MORPH_OPEN, kernel)', True, 'import cv2 as cv\n'), ((107, 14, 107, 35), 'cv2.medianBlur', 'cv.medianBlur', ({(107, 28, 107, 31): 'img', (107, 33, 107, 34): '3'}, {}), '(img, 3)', True, 'import cv2 as cv\n'), ((116, 30, 116, 86), 'cv2.findContours', 'cv.findContours', ({(116, 46, 116, 49): 'img', (116, 51, 116, 63): 'cv.RETR_LIST', (116, 65, 116, 85): 'cv.CHAIN_APPROX_NONE'}, {}), '(img, cv.RETR_LIST, cv.CHAIN_APPROX_NONE)', True, 'import cv2 as cv\n'), ((149, 8, 149, 55), 'cv2.circle', 'cv.circle', ({(149, 18, 149, 26): 'img_copy', (149, 28, 149, 34): 'origin', (149, 36, 149, 37): '(4)', (149, 39, 149, 50): '(0, 0, 255)', (149, 52, 149, 54): '(-1)'}, {}), '(img_copy, origin, 4, (0, 0, 255), -1)', True, 'import cv2 as cv\n'), ((155, 8, 155, 36), 'cv2.imshow', 'cv.imshow', ({(155, 18, 155, 25): '"""Final"""', (155, 27, 155, 35): 'img_copy'}, {}), "('Final', img_copy)", True, 'import cv2 as cv\n'), ((123, 21, 123, 64), 'cv2.approxPolyDP', 'cv.approxPolyDP', ({(123, 37, 123, 48): 'contours[i]', (123, 50, 123, 57): 'epsilon', (123, 59, 123, 63): 'True'}, {}), '(contours[i], epsilon, True)', True, 'import cv2 as cv\n'), ((125, 34, 125, 57), 'cv2.boundingRect', 'cv.boundingRect', ({(125, 50, 125, 56): 'approx'}, {}), '(approx)', True, 'import cv2 as cv\n'), ((158, 11, 158, 25), 'cv2.waitKey', 'cv.waitKey', ({(158, 22, 158, 24): '(30)'}, {}), '(30)', True, 'import cv2 as cv\n'), ((122, 31, 122, 62), 'cv2.arcLength', 'cv.arcLength', ({(122, 44, 122, 55): 'contours[i]', (122, 57, 122, 61): '(True)'}, {}), '(contours[i], True)', True, 'import cv2 as cv\n'), ((130, 33, 130, 62), 'cv2.minEnclosingCircle', 'cv.minEnclosingCircle', ({(130, 55, 130, 61): 'approx'}, {}), '(approx)', True, 'import cv2 as cv\n'), ((140, 16, 140, 67), 'cv2.circle', 'cv.circle', ({(140, 26, 140, 34): 'img_copy', (140, 36, 140, 42): 'center', (140, 44, 140, 50): 'radius', (140, 52, 140, 63): '(0, 255, 0)', (140, 65, 140, 66): '(2)'}, {}), '(img_copy, center, radius, (0, 255, 0), 2)', True, 'import cv2 as cv\n'), ((117, 46, 117, 63), 'cv2.contourArea', 'cv.contourArea', ({(117, 61, 117, 62): 'x'}, {}), '(x)', True, 'import cv2 as cv\n'), ((143, 47, 143, 63), 'math.hypot', 'math.hypot', ({(143, 58, 143, 59): 'X', (143, 61, 143, 62): 'Y'}, {}), '(X, Y)', False, 'import math\n')]