import cv2
import numpy as np

from matrices_reader import *

VOXEL_SIZE = 1e-3
X_MIN, X_MAX = -2.0, 2.0
Y_MIN, Y_MAX = -2.0, 2.0
Z_MIN, Z_MAX = -2.0, 2.0

# grid = [[[
#     1 for z in np.arange(Z_MIN, Z_MAX, VOXEL_SIZE)
#     ] for y in np.arange(Y_MIN, Y_MAX, VOXEL_SIZE)
#     ] for x in np.arange(X_MIN, X_MAX, VOXEL_SIZE)
# ]

projection_matrices = matrices_reader('data/torus/matrices.txt')
nb_frame = len(projection_matrices)
point = np.array([1.0, 0.0, 0.0, 1.0])

for k in range(nb_frame):
    
    proj_mat = projection_matrices[k]
    cam_point = proj_mat @ point
    cam_point /= cam_point[2]

    frame = cv2.imread(f'data/torus/torus{k+1:04}.png')
    cv2.circle(frame, (int(cam_point[0]), int(cam_point[1])), 2, (0, 0, 255))
    cv2.imshow('Frame', frame)
    cv2.waitKey(0)