import cv2
import os
import sys

sys.path.append(os.path.abspath(os.path.join(os.path.dirname(__file__), '..')))
from default_config import *


visualization = True
CHESS_BOARD_DIM = (9, 6)
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)

# nếu tồn tại thư mục ảnh, xóa đi và tạo lại
image_dir_path = f"camera_calibration/images/{set_resolution}"
if os.path.exists(image_dir_path):
    import shutil
    shutil.rmtree(image_dir_path)
os.makedirs(image_dir_path)

def detect_checker_board(image, grayImage, criteria, boardDimension):
    ret, corners = cv2.findChessboardCorners(grayImage, boardDimension)
    if ret == True:
        corners1 = cv2.cornerSubPix(grayImage, corners, (3, 3), (-1, -1), criteria)
        image = cv2.drawChessboardCorners(image, boardDimension, corners1, ret)

    return image, ret


cap = cv2.VideoCapture(0)

# Thay đổi độ phân giải phù hợp với yêu cầu ban đầu
new_height = RESOLUTION_STANDARDS[set_resolution]
new_width, new_height = change_resolution(cap,new_height)    
print(f'Resolution camera {new_width}x{new_height}')

n = 0  # image_counter
while True:
    _, frame = cap.read()
    copyFrame = frame.copy()
    gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)

    image, board_detected = detect_checker_board(frame, gray, criteria, CHESS_BOARD_DIM)

    if visualization:
        cv2.putText(
            frame,
            f"saved_img : {n}",
            (30, 40),
            cv2.FONT_HERSHEY_PLAIN,
            1.4,
            (0, 255, 0),
            2,
            cv2.LINE_AA,
        )

        cv2.imshow("frame", frame)
        cv2.imshow("copyFrame", copyFrame)

    key = cv2.waitKey(1)

    if key == ord("q"):
        break
    if key == ord("s") and board_detected == True:
        # storing the checker board image
        cv2.imwrite(f"{image_dir_path}/image{n}.png", copyFrame)

        print(f"saved image number {n}")
        n += 1  # incrementing the image counter
cap.release()
cv2.destroyAllWindows()

print("Total saved Images:", n)