import numpy as np
from numpy.core.multiarray import dtype

CAMERA_MATRIX = np.load('camera_parameters.npy', allow_pickle=True).item()['K']

W, H = 1280, 720
FAR, NEAR = 100, 1

def build_opengl_projection_for_intrinsics(intrinsics, w, h, far, near):
    left = 0
    right = w
    bottom = h
    top = 0
    print("Instrinsics")
    print(intrinsics)
    print()
    
    ortho = np.array([
        [  2/(right-left),                  0,              0,  -(right+left)/(right-left)],
        [               0,     2/(top-bottom),              0,  -(top+bottom)/(top-bottom)],
        [               0,                  0,  -2/(far-near),      -(far+near)/(far-near)],
        [               0,                  0,              0,                           1],
    ], dtype=np.float32)
    print("Ortho")
    print(ortho)
    print()

    persp = np.array([
        [   intrinsics[0][0],       intrinsics[0][1],       -intrinsics[0][2],           0],
        [                  0,       intrinsics[1][1],       -intrinsics[1][2],           0],
        [                  0,                      0,            (near+far), (near*far)],
        [                  0,                      0,                      -1,           0],
    ], dtype=np.float32)
    print("Persp")
    print(persp)
    print()

    print("ANS")
    print(ortho @ persp)
    print()
    return ortho @ persp