import cv2
import os
import sys
import time

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

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

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:
        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 video theo cài đặt ở file default_config.py
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
MAX_IMAGES = 20  # Limit for automatic saving

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

    copyFrame = frame.copy()
    gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)

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

    if board_detected:
        save_path = f"{image_dir_path}/image{n}.png"
        cv2.imwrite(save_path, copyFrame)
        print(f"[INFO] Saved image {n} at {save_path}")
        n += 1

        if n >= MAX_IMAGES:
            break
        time.sleep(5)

cap.release()
print("Total saved images:", n)