# Basic OBJ file viewer. needs objloader from:
#  http://www.pygame.org/wiki/OBJFileLoader
# LMB + move: rotate
# RMB + move: pan
# Scroll wheel: zoom in/out
import sys, pygame
from pygame.locals import *
from pygame.constants import *
from OpenGL.GL import *
from OpenGL.GLU import *

# IMPORT OBJECT LOADER
from objloader import *
from projection_matrix import build_opengl_projection_for_intrinsics
import numpy as np

CAMERA_MATRIX = np.load('camera_parameters.npy', allow_pickle=True).item()['K']
EXTRINSIC_MATRIX = np.array([[0.7227021503496043, -0.3686776760226368, 0.5846181429639397, -16.218840163220325], [0.44397563400870765, 0.8958935447266068, 0.016136694945417585, -1.2057431225855768], [-0.5297048596026128, 0.24789418653881962, 0.8111480962152553, 49.33418176483625], [0.0, 0.0, 0.0, 1.0]])
W, H = 1280, 720
FAR, NEAR = 10000.0, 1.0


if __name__ == '__main__':
    proj = build_opengl_projection_for_intrinsics(CAMERA_MATRIX, W, H, FAR, NEAR)

    pygame.init()
    viewport = (W, H)
    hx = viewport[0]/2
    hy = viewport[1]/2
    srf = pygame.display.set_mode(viewport, OPENGL | DOUBLEBUF)

    # glLightfv(GL_LIGHT0, GL_POSITION,  (-40, 200, 100, 0.0))
    # glLightfv(GL_LIGHT0, GL_AMBIENT, (0.2, 0.2, 0.2, 1.0))
    # glLightfv(GL_LIGHT0, GL_DIFFUSE, (0.5, 0.5, 0.5, 1.0))
    glEnable(GL_LIGHT0)
    glEnable(GL_LIGHTING)
    glEnable(GL_COLOR_MATERIAL)
    glEnable(GL_DEPTH_TEST)
    glShadeModel(GL_SMOOTH)           # most obj files expect to be smooth-shaded

    # LOAD OBJECT AFTER PYGAME INIT
    obj = OBJ(sys.argv[1], swapyz=True)

    clock = pygame.time.Clock()

    # set projection matrix
    # use intrinsics to build for OpenGL
    glMatrixMode(GL_PROJECTION)
    glLoadIdentity()
    # width, height = viewport
    # gluPerspective(90.0, width/float(height), 1, 100.0)
    print(proj)
    glLoadMatrixf(proj)
    glEnable(GL_DEPTH_TEST)

    
    # set model view matrix
    # use extrinsics
    glMatrixMode(GL_MODELVIEW)

    rx, ry = (0,0)
    tx, ty = (0,0)
    zpos = 5
    rotate = move = False
    while 1:
        clock.tick(30)
        for e in pygame.event.get():
            if e.type == QUIT:
                sys.exit()
            elif e.type == KEYDOWN and e.key == K_ESCAPE:
                sys.exit()
            elif e.type == MOUSEBUTTONDOWN:
                if e.button == 4: zpos = max(1, zpos-1)
                elif e.button == 5: zpos += 1
                elif e.button == 1: rotate = True
                elif e.button == 3: move = True
            elif e.type == MOUSEBUTTONUP:
                if e.button == 1: rotate = False
                elif e.button == 3: move = False
            elif e.type == MOUSEMOTION:
                i, j = e.rel
                if rotate:
                    rx += i
                    ry += j
                if move:
                    tx += i
                    ty -= j

        glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT)
        glMatrixMode(GL_MODELVIEW)
        glLoadIdentity()
        glLoadMatrixf(EXTRINSIC_MATRIX)

        # RENDER OBJECT
        '''
        glTranslate(tx/20., ty/20., - zpos)
        size = 3
        glScale(size, size, size)
        glRotate(ry, 1, 0, 0)
        glRotate(rx, 0, 1, 0)
        '''
        glCallList(obj.gl_list)

        pygame.display.flip()