import cv2
import numpy as np
import matplotlib.pyplot as plt
from copy import deepcopy

from image import *

CAMERA_CALIBRATION_FRAMES = 50

class Camera():
    def __init__(self, frames):
        self._mat = None
        self.calibrate(frames)

    def getIntrinsicMatrix(self):
        return self._mat

    def calibrate(self, frames):
        """Fonte: https://docs.opencv.org/3.4.1/dc/dbb/tutorial_py_calibration.html"""

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

        # prepare object points, like (0,0,0), (1,0,0), (2,0,0) ....,(6,5,0)
        objp = np.zeros((8*6,3), np.float32)
        objp[:,:2] = np.mgrid[0:8,0:6].T.reshape(-1,2)

        # Arrays to store object points and image points from all the images.
        objpoints = [] # 3d point in real world space
        imgpoints = [] # 2d points in image plane.

        i = 0
        print("Calibration Started!")
        while i < CAMERA_CALIBRATION_FRAMES:
            gray = frames[i].getGrayScale()

            corners = gray.getChessBoardCorners(8,6)
            #cv2.findChessboardCorners(gray, (8, 6), None)
            #print("hmm")
            if corners != None:
                i += 1
                print(i,"/", CAMERA_CALIBRATION_FRAMES)

                objpoints.append(objp)
                corners2 = cv2.cornerSubPix(gray, corners, (11,11), (-1,-1), criteria)
                imgpoints.append(corners)

        ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(objpoints, imgpoints, gray.shape[::-1], None, None)

        self._mat = mtx