/*
 * OpenHMD - Free and Open Source API and drivers for immersive technology.
 * Copyright (C) 2013 Fredrik Hultin.
 * Copyright (C) 2013 Jakob Bornecrantz.
 * Distributed under the Boost 1.0 licence, see LICENSE for full text.
 */

/* OpenGL Test - Main Implementation */

#include <openhmd.h>
#include <stdbool.h>
#include <assert.h>
#include <math.h>
#include "gl.h"

#include <stdio.h>
#include <stdlib.h>

#include <lvrc.h>

#define OVERSAMPLE_SCALE 2.0

float randf()
{
	return (float)rand() / (float)RAND_MAX;
}

GLuint gen_cubes()
{
	GLuint list = glGenLists(1);

	// Set the random seed.
	srand(42);

	glNewList(list, GL_COMPILE);

	for(float a = 0.0f; a < 360.0f; a += 20.0f){
		glPushMatrix();

		glRotatef(a, 0, 1, 0);
		glTranslatef(0, 0, -1);
		glScalef(0.2, 0.2, 0.2);
		glRotatef(randf() * 360, randf(), randf(), randf());

		glColor4f(randf(), randf(), randf(), randf() * .5f + .5f);
		draw_cube();

		glPopMatrix();
	}

	// draw floor
	glColor4f(0, 1.0f, .25f, .25f);
	glTranslatef(0, -2.5f, 0);
	draw_cube();

	glEndList();

	return list;
}

void draw_crosshairs(float len, float cx, float cy)
{
	glMatrixMode(GL_MODELVIEW);
	glPushMatrix();
	glLoadIdentity();
	glMatrixMode(GL_PROJECTION);
	glPushMatrix();
	glLoadIdentity();
	glBegin(GL_LINES);
	float l = len/2.0f;
	glVertex3f(cx - l, cy, 0.0);
	glVertex3f(cx + l, cy, 0.0);
	glVertex3f(cx, cy - l, 0.0);
	glVertex3f(cx, cy + l, 0.0);
	glEnd();
	glMatrixMode(GL_PROJECTION);
	glPopMatrix();
	glMatrixMode(GL_MODELVIEW);
	glPopMatrix();
}

void draw_scene(GLuint list)
{
	// draw cubes
	glCallList(list);
}
static inline void
print_matrix(float m[])
{
	printf("[[%0.4f, %0.4f, %0.4f, %0.4f],\n"
			"[%0.4f, %0.4f, %0.4f, %0.4f],\n"
			"[%0.4f, %0.4f, %0.4f, %0.4f],\n"
			"[%0.4f, %0.4f, %0.4f, %0.4f]]\n",
			m[0], m[4], m[8], m[12],
			m[1], m[5], m[9], m[13],
			m[2], m[6], m[10], m[14],
			m[3], m[7], m[11], m[15]);
}

int main(int argc, char** argv)
{
	int hmd_w, hmd_h;

	ohmd_context* ctx = ohmd_ctx_create();
	int num_devices = ohmd_ctx_probe(ctx);
	if(num_devices < 0){
		printf("failed to probe devices: %s\n", ohmd_ctx_get_error(ctx));
		return 1;
	}

	ohmd_device_settings* settings = ohmd_device_settings_create(ctx);

	// If OHMD_IDS_AUTOMATIC_UPDATE is set to 0, ohmd_ctx_update() must be called at least 10 times per second.
	// It is enabled by default.

	int auto_update = 1;
	ohmd_device_settings_seti(settings, OHMD_IDS_AUTOMATIC_UPDATE, &auto_update);

	ohmd_device* hmd = ohmd_list_open_device_s(ctx, 0, settings);
	if(!hmd){
		printf("failed to open device: %s\n", ohmd_ctx_get_error(ctx));
		return 1;
	}
	ohmd_device_geti(hmd, OHMD_SCREEN_HORIZONTAL_RESOLUTION, &hmd_w);
	ohmd_device_geti(hmd, OHMD_SCREEN_VERTICAL_RESOLUTION, &hmd_h);
	float ipd;
	ohmd_device_getf(hmd, OHMD_EYE_IPD, &ipd);
	float viewport_scale[2];
	float distortion_coeffs[4];
	float aberr_scale[3];
	float sep;
	float left_lens_center[2];
	float right_lens_center[2];
	//viewport is half the screen
	ohmd_device_getf(hmd, OHMD_SCREEN_HORIZONTAL_SIZE, &(viewport_scale[0]));
	viewport_scale[0] /= 2.0f;
	ohmd_device_getf(hmd, OHMD_SCREEN_VERTICAL_SIZE, &(viewport_scale[1]));
	//distortion coefficients
	ohmd_device_getf(hmd, OHMD_UNIVERSAL_DISTORTION_K, &(distortion_coeffs[0]));
	ohmd_device_getf(hmd, OHMD_UNIVERSAL_ABERRATION_K, &(aberr_scale[0]));
	//calculate lens centers (assuming the eye separation is the distance between the lens centers)
	ohmd_device_getf(hmd, OHMD_LENS_HORIZONTAL_SEPARATION, &sep);
	ohmd_device_getf(hmd, OHMD_LENS_VERTICAL_POSITION, &(left_lens_center[1]));
	ohmd_device_getf(hmd, OHMD_LENS_VERTICAL_POSITION, &(right_lens_center[1]));
	left_lens_center[0] = viewport_scale[0] - sep/2.0f;
	right_lens_center[0] = sep/2.0f;
	//assume calibration was for lens view to which ever edge of screen is further away from lens center
	float warp_scale = (left_lens_center[0] > right_lens_center[0]) ? left_lens_center[0] : right_lens_center[0];
	float warp_adj = 1.0f;

	ohmd_device_settings_destroy(settings);

	struct lvrcInstance * compositor = lvrcCreateInstance(hmd);

	gl_ctx gl;
	init_gl(&gl, hmd_w, hmd_h);

	GLuint list = gen_cubes();

	lvrcInitRenderingEGL(compositor, gl.egl_display, gl.egl_context, gl.egl_surface);

	int eye_w = hmd_w/2*OVERSAMPLE_SCALE;
	int eye_h = hmd_h*OVERSAMPLE_SCALE;

	GLuint textures [2];

	bool bSwapChainInit = lvrcInitSwapChain(compositor, eye_w, eye_h, textures);

	if (!bSwapChainInit)
	{
		return -1;
	}

	GLuint left_color_tex = textures[0], left_depth_tex = 0, left_fbo = 0;
	create_fbo(eye_w, eye_h, &left_fbo, left_color_tex, &left_depth_tex);

	GLuint right_color_tex = textures[1], right_depth_tex = 0, right_fbo = 0;
	create_fbo(eye_w, eye_h, &right_fbo, right_color_tex, &right_depth_tex);

	bool done = false;
	bool crosshair_overlay = false;
	while(!done){

		ohmd_ctx_update(ctx);

		lvrcBeginFrame(compositor);

		eglMakeCurrent(gl.egl_display, gl.egl_surface, gl.egl_surface, gl.egl_context);

		// Common scene state
		glEnable(GL_BLEND);
		glEnable(GL_DEPTH_TEST);
		float matrix[16];

		// set hmd rotation, for left eye.
		glMatrixMode(GL_PROJECTION);
		ohmd_device_getf(hmd, OHMD_LEFT_EYE_GL_PROJECTION_MATRIX, matrix);
		glLoadMatrixf(matrix);

		glMatrixMode(GL_MODELVIEW);
		ohmd_device_getf(hmd, OHMD_LEFT_EYE_GL_MODELVIEW_MATRIX, matrix);
		glLoadMatrixf(matrix);

		// Draw scene into framebuffer.
		glBindFramebuffer(GL_FRAMEBUFFER_EXT, left_fbo);
		glViewport(0, 0, eye_w, eye_h);
		glClearColor(0.0, 0.0, 0.0, 1.0);
		glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
		draw_scene(list);
		if (crosshair_overlay) {
			glClear(GL_DEPTH_BUFFER_BIT);
			glLineWidth(2.0*OVERSAMPLE_SCALE);
			glColor4f(1.0, 0.5, 0.0, 1.0);
			draw_crosshairs(0.1, 2*left_lens_center[0]/viewport_scale[0] - 1.0f, 2*left_lens_center[1]/viewport_scale[1] - 1.0f);
		}

		// set hmd rotation, for right eye.
		glMatrixMode(GL_PROJECTION);
		ohmd_device_getf(hmd, OHMD_RIGHT_EYE_GL_PROJECTION_MATRIX, matrix);
		glLoadMatrixf(matrix);

		glMatrixMode(GL_MODELVIEW);
		ohmd_device_getf(hmd, OHMD_RIGHT_EYE_GL_MODELVIEW_MATRIX, matrix);
		glLoadMatrixf(matrix);

		// Draw scene into framebuffer.
		glBindFramebuffer(GL_FRAMEBUFFER_EXT, right_fbo);
		glViewport(0, 0, eye_w, eye_h);
		glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
		draw_scene(list);
		if (crosshair_overlay) {
			glClear(GL_DEPTH_BUFFER_BIT);
			glLineWidth(5.0);
			glColor4f(1.0, 0.5, 0.0, 1.0);
			draw_crosshairs(0.1, 2*right_lens_center[0]/viewport_scale[0] - 1.0f, 2*right_lens_center[1]/viewport_scale[1] - 1.0f);
		}

		// Clean up common draw state
		glBindFramebuffer(GL_FRAMEBUFFER_EXT, 0);
		glDisable(GL_BLEND);
		glDisable(GL_DEPTH_TEST);

		// Setup ortho state.
		glViewport(0, 0, hmd_w, hmd_h);
		glEnable(GL_TEXTURE_2D);
		glColor4d(1, 1, 1, 1);

		// Setup simple render state
		glMatrixMode(GL_PROJECTION);
		glLoadIdentity();
		glMatrixMode(GL_MODELVIEW);
		glLoadIdentity();

		// Draw left eye
		glBindTexture(GL_TEXTURE_2D, left_color_tex);
		glBegin(GL_QUADS);
		glTexCoord2d( 0,  0);
		glVertex3d(  -1, -1, 0);
		glTexCoord2d( 1,  0);
		glVertex3d(   0, -1, 0);
		glTexCoord2d( 1,  1);
		glVertex3d(   0,  1, 0);
		glTexCoord2d( 0,  1);
		glVertex3d(  -1,  1, 0);
		glEnd();

		// Draw right eye
		glBindTexture(GL_TEXTURE_2D, right_color_tex);
		glBegin(GL_QUADS);
		glTexCoord2d( 0,  0);
		glVertex3d(   0, -1, 0);
		glTexCoord2d( 1,  0);
		glVertex3d(   1, -1, 0);
		glTexCoord2d( 1,  1);
		glVertex3d(   1,  1, 0);
		glTexCoord2d( 0,  1);
		glVertex3d(   0,  1, 0);
		glEnd();

		// Clean up state.
		glBindTexture(GL_TEXTURE_2D, 0);
		glDisable(GL_TEXTURE_2D);
		glUseProgram(0);

		glFinish();

		// Da swap-dawup!
		eglSwapBuffers(gl.egl_display, gl.egl_surface);

		lvrcEndFrame(compositor);
	}

	lvrcDestroyInstance(compositor);

	ohmd_ctx_destroy(ctx);

	return 0;
}