#include "bcm_host.h"
#include "GLES/gl.h"
#include "EGL/egl.h"
#include "EGL/eglext.h"
#include "GLES2/gl2.h"
#include <assert.h>
#include <stdio.h>
#include <stdlib.h>
#include <memory.h>

static int op_filtering = 0;
static float op_zoom = 1.0f;

#define	SHOW_ERROR		/*gles_show_error();*/

static const char* vertex_shader =
	"uniform mat4 u_vp_matrix;								\n"
	"attribute vec4 a_position;								\n"
	"attribute vec2 a_texcoord;								\n"
	"varying mediump vec2 v_texcoord;						\n"
	"void main()											\n"
	"{														\n"
	"	v_texcoord = a_texcoord;							\n"
	"	gl_Position = u_vp_matrix * a_position;				\n"
	"}														\n";

static const char* fragment_shader =
	"varying mediump vec2 v_texcoord;												\n"
	"uniform sampler2D u_texture;													\n"
	"uniform sampler2D u_palette;													\n"
	"void main()																	\n"
	"{																				\n"
	"	vec4 p0 = texture2D(u_texture, v_texcoord);									\n" // use paletted texture
	"	vec4 c0 = texture2D(u_palette, vec2(p0.r + 1.0/256.0*0.5, 0.5)); 		\n"
	"	gl_FragColor = c0;											 				\n"
	"}																				\n";

static const char* fragment_shader_filtering =
	"varying mediump vec2 v_texcoord;												\n"
	"uniform sampler2D u_texture;													\n"
	"uniform sampler2D u_palette;													\n"
	"void main()																	\n"
	"{																				\n"
	"	vec4 p0 = texture2D(u_texture, v_texcoord);									\n" // manually linear filtering of paletted texture is awful
	"	vec4 p1 = texture2D(u_texture, v_texcoord + vec2(1.0/512.0, 0)); 			\n"
	"	vec4 p2 = texture2D(u_texture, v_texcoord + vec2(0, 1.0/256.0)); 			\n"
	"	vec4 p3 = texture2D(u_texture, v_texcoord + vec2(1.0/512.0, 1.0/256.0)); 	\n"
	"	vec4 c0 = texture2D(u_palette, vec2(p0.r + 1.0/256.0*0.5, 0.5)); 			\n"
	"	vec4 c1 = texture2D(u_palette, vec2(p1.r + 1.0/256.0*0.5, 0.5)); 			\n"
	"	vec4 c2 = texture2D(u_palette, vec2(p2.r + 1.0/256.0*0.5, 0.5)); 			\n"
	"	vec4 c3 = texture2D(u_palette, vec2(p3.r + 1.0/256.0*0.5, 0.5)); 			\n"
	"	vec2 l = vec2(fract(512.0*v_texcoord.x), fract(256.0*v_texcoord.y)); 		\n"
	"	gl_FragColor = mix(mix(c0, c1, l.x), mix(c2, c3, l.x), l.y); 				\n"
	"}																				\n";

static const GLfloat vertices[] =
{
	-0.5f, -0.5f, 0.0f,
	+0.5f, -0.5f, 0.0f,
	+0.5f, +0.5f, 0.0f,
	-0.5f, +0.5f, 0.0f,
};

#define	TEX_WIDTH	1024
#define	TEX_HEIGHT	512
#define	WIDTH		640
#define	HEIGHT		400

#define	min_u		0.0f /*24.0f/TEX_WIDTH*/
#define	max_u		(float)WIDTH/TEX_WIDTH /*- min_u*/
#define	min_v		0.0f
#define	max_v		(float)HEIGHT/TEX_HEIGHT

static const GLfloat uvs[] =
{
	min_u, min_v,
	max_u, min_v,
	max_u, max_v,
	min_u, max_v,
};

static const GLushort indices[] =
{
	0, 1, 2,
	0, 2, 3,
};

static const int kVertexCount = 4;
static const int kIndexCount = 6;

void gles_show_error()
{
	GLenum error = GL_NO_ERROR;
    error = glGetError();
    if (GL_NO_ERROR != error)
        printf("GL Error %x encountered!\n", error);
}

static GLuint CreateShader(GLenum type, const char *shader_src)
{
	GLuint shader = glCreateShader(type);
	if(!shader)
		return 0;

	// Load and compile the shader source
	glShaderSource(shader, 1, &shader_src, NULL);
	glCompileShader(shader);

	// Check the compile status
	GLint compiled = 0;
	glGetShaderiv(shader, GL_COMPILE_STATUS, &compiled);
	if(!compiled)
	{
		GLint info_len = 0;
		glGetShaderiv(shader, GL_INFO_LOG_LENGTH, &info_len);
		if(info_len > 1)
		{
			char* info_log = (char *)malloc(sizeof(char) * info_len);
			glGetShaderInfoLog(shader, info_len, NULL, info_log);
			// TODO(dspringer): We could really use a logging API.
			printf("Error compiling shader:\n%s\n", info_log);
			free(info_log);
		}
		glDeleteShader(shader);
		return 0;
	}
	return shader;
}

static GLuint CreateProgram(const char *vertex_shader_src, const char *fragment_shader_src)
{
	GLuint vertex_shader = CreateShader(GL_VERTEX_SHADER, vertex_shader_src);
	if(!vertex_shader)
		return 0;
	GLuint fragment_shader = CreateShader(GL_FRAGMENT_SHADER, fragment_shader_src);
	if(!fragment_shader)
	{
		glDeleteShader(vertex_shader);
		return 0;
	}

	GLuint program_object = glCreateProgram();
	if(!program_object)
		return 0;
	glAttachShader(program_object, vertex_shader);
	glAttachShader(program_object, fragment_shader);

	// Link the program
	glLinkProgram(program_object);

	// Check the link status
	GLint linked = 0;
	glGetProgramiv(program_object, GL_LINK_STATUS, &linked);
	if(!linked)
	{
		GLint info_len = 0;
		glGetProgramiv(program_object, GL_INFO_LOG_LENGTH, &info_len);
		if(info_len > 1)
		{
			char* info_log = (char *)malloc(info_len);
			glGetProgramInfoLog(program_object, info_len, NULL, info_log);
			// TODO(dspringer): We could really use a logging API.
			printf("Error linking program:\n%s\n", info_log);
			free(info_log);
		}
		glDeleteProgram(program_object);
		return 0;
	}
	// Delete these here because they are attached to the program object.
	glDeleteShader(vertex_shader);
	glDeleteShader(fragment_shader);
	return program_object;
}

typedef	struct ShaderInfo {
		GLuint program;
		GLint a_position;
		GLint a_texcoord;
		GLint u_vp_matrix;
		GLint u_texture;
		GLint u_palette;
} ShaderInfo;

static ShaderInfo shader;
static ShaderInfo shader_filtering;
static GLuint buffers[3];
static GLuint textures[2];
static char palette_changed;

static void gles2_create()
{
	memset(&shader, 0, sizeof(ShaderInfo));
	shader.program = CreateProgram(vertex_shader, fragment_shader);
	if(shader.program)
	{
		shader.a_position	= glGetAttribLocation(shader.program,	"a_position");
		shader.a_texcoord	= glGetAttribLocation(shader.program,	"a_texcoord");
		shader.u_vp_matrix	= glGetUniformLocation(shader.program,	"u_vp_matrix");
		shader.u_texture	= glGetUniformLocation(shader.program,	"u_texture");
		shader.u_palette	= glGetUniformLocation(shader.program,	"u_palette");
	}
	memset(&shader_filtering, 0, sizeof(ShaderInfo));
	shader_filtering.program = CreateProgram(vertex_shader, fragment_shader_filtering);
	if(shader_filtering.program)
	{
		shader_filtering.a_position	= glGetAttribLocation(shader_filtering.program,	"a_position");
		shader_filtering.a_texcoord	= glGetAttribLocation(shader_filtering.program,	"a_texcoord");
		shader_filtering.u_vp_matrix	= glGetUniformLocation(shader_filtering.program,	"u_vp_matrix");
		shader_filtering.u_texture	= glGetUniformLocation(shader_filtering.program,	"u_texture");
		shader_filtering.u_palette	= glGetUniformLocation(shader_filtering.program,	"u_palette");
	}
	palette_changed = 1;
	if(!shader.program || !shader_filtering.program)
		return;

	glGenTextures(2, textures);	SHOW_ERROR
	glBindTexture(GL_TEXTURE_2D, textures[0]); SHOW_ERROR
	glTexImage2D(GL_TEXTURE_2D, 0, GL_LUMINANCE, TEX_WIDTH, TEX_HEIGHT, 0, GL_LUMINANCE, GL_UNSIGNED_BYTE, NULL); SHOW_ERROR
	glBindTexture(GL_TEXTURE_2D, textures[1]); SHOW_ERROR	// color palette
	glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, 256, 1, 0, GL_RGB, GL_UNSIGNED_SHORT_5_6_5, NULL); SHOW_ERROR

	glGenBuffers(3, buffers); SHOW_ERROR
	glBindBuffer(GL_ARRAY_BUFFER, buffers[0]); SHOW_ERROR
	glBufferData(GL_ARRAY_BUFFER, kVertexCount * sizeof(GLfloat) * 3, vertices, GL_STATIC_DRAW); SHOW_ERROR
	glBindBuffer(GL_ARRAY_BUFFER, buffers[1]); SHOW_ERROR
	glBufferData(GL_ARRAY_BUFFER, kVertexCount * sizeof(GLfloat) * 2, uvs, GL_STATIC_DRAW); SHOW_ERROR
	glBindBuffer(GL_ARRAY_BUFFER, 0); SHOW_ERROR
	glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, buffers[2]); SHOW_ERROR
	glBufferData(GL_ELEMENT_ARRAY_BUFFER, kIndexCount * sizeof(GL_UNSIGNED_SHORT), indices, GL_STATIC_DRAW); SHOW_ERROR
	glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, 0); SHOW_ERROR

	glClearColor(0.0f, 0.0f, 0.0f, 1.0f); SHOW_ERROR
	glDisable(GL_DEPTH_TEST); SHOW_ERROR
	glDisable(GL_DITHER); SHOW_ERROR
}

static uint32_t screen_width = 0;
static uint32_t screen_height = 0;

static EGLDisplay display = NULL;
static EGLSurface surface = NULL;
static EGLContext context = NULL;
static EGL_DISPMANX_WINDOW_T nativewindow;

void gles_init()
{
	bcm_host_init();

	// get an EGL display connection
	display = eglGetDisplay(EGL_DEFAULT_DISPLAY);
	assert(display != EGL_NO_DISPLAY);

	// initialize the EGL display connection
	EGLBoolean result = eglInitialize(display, NULL, NULL);
	assert(EGL_FALSE != result);

	// get an appropriate EGL frame buffer configuration
	EGLint num_config;
	EGLConfig config;
	static const EGLint attribute_list[] =
	{
		EGL_RED_SIZE, 8,
		EGL_GREEN_SIZE, 8,
		EGL_BLUE_SIZE, 8,
		EGL_ALPHA_SIZE, 8,
		EGL_SURFACE_TYPE, EGL_WINDOW_BIT,
		EGL_NONE
	};
	result = eglChooseConfig(display, attribute_list, &config, 1, &num_config);
	assert(EGL_FALSE != result);

	result = eglBindAPI(EGL_OPENGL_ES_API);
	assert(EGL_FALSE != result);

	// create an EGL rendering context
	static const EGLint context_attributes[] =
	{
		EGL_CONTEXT_CLIENT_VERSION, 2,
		EGL_NONE
	};
	context = eglCreateContext(display, config, EGL_NO_CONTEXT, context_attributes);
	assert(context != EGL_NO_CONTEXT);

	// create an EGL window surface
	int32_t success = graphics_get_display_size(0, &screen_width, &screen_height);
	assert(success >= 0);

	screen_width = 640;
	screen_height = 400;

	VC_RECT_T dst_rect;
	dst_rect.x = 0;
	dst_rect.y = 0;
	dst_rect.width = screen_width;
	dst_rect.height = screen_height;

	VC_RECT_T src_rect;
	src_rect.x = 0;
	src_rect.y = 0;
	src_rect.width = screen_width << 16;
	src_rect.height = screen_height << 16;

	DISPMANX_DISPLAY_HANDLE_T dispman_display = vc_dispmanx_display_open(0);
	DISPMANX_UPDATE_HANDLE_T dispman_update = vc_dispmanx_update_start(0);
	DISPMANX_ELEMENT_HANDLE_T dispman_element = vc_dispmanx_element_add(dispman_update, dispman_display,
	  0, &dst_rect, 0, &src_rect, DISPMANX_PROTECTION_NONE, NULL, NULL, DISPMANX_NO_ROTATE);

	nativewindow.element = dispman_element;
	nativewindow.width = screen_width;
	nativewindow.height = screen_height;
	vc_dispmanx_update_submit_sync(dispman_update);

	surface = eglCreateWindowSurface(display, config, &nativewindow, NULL);
	assert(surface != EGL_NO_SURFACE);

	// connect the context to the surface
	result = eglMakeCurrent(display, surface, surface, context);
	assert(EGL_FALSE != result);

	gles2_create();
}

static void gles2_destroy()
{
	if(shader.program)
		glDeleteProgram(shader.program);
	if(shader_filtering.program)
		glDeleteProgram(shader_filtering.program);
	if(!shader.program || !shader_filtering.program)
		return;
	glDeleteBuffers(3, buffers); SHOW_ERROR
	glDeleteTextures(2, textures); SHOW_ERROR
}

static void SetOrtho(float m[4][4], float left, float right, float bottom, float top, float near, float far, float scale_x, float scale_y)
{
	memset(m, 0, 4*4*sizeof(float));
	m[0][0] = 2.0f/(right - left)*scale_x;
	m[1][1] = 2.0f/(top - bottom)*scale_y;
	m[2][2] = -2.0f/(far - near);
	m[3][0] = -(right + left)/(right - left);
	m[3][1] = -(top + bottom)/(top - bottom);
	m[3][2] = -(far + near)/(far - near);
	m[3][3] = 1;
}

static void gles2_DrawQuad(const ShaderInfo *sh, GLuint textures[2])
{
	glUniform1i(sh->u_texture, 0); SHOW_ERROR
	glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST); SHOW_ERROR //@note : GL_LINEAR must be implemented in shader because of palette indexes in texture
	glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST); SHOW_ERROR

	glActiveTexture(GL_TEXTURE1); SHOW_ERROR
    glBindTexture(GL_TEXTURE_2D, textures[1]); SHOW_ERROR
	glUniform1i(sh->u_palette, 1); SHOW_ERROR
	glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST); SHOW_ERROR
	glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST); SHOW_ERROR

	glBindBuffer(GL_ARRAY_BUFFER, buffers[0]); SHOW_ERROR
	glVertexAttribPointer(sh->a_position, 3, GL_FLOAT, GL_FALSE, 3 * sizeof(GLfloat), NULL); SHOW_ERROR
	glEnableVertexAttribArray(sh->a_position); SHOW_ERROR

	glBindBuffer(GL_ARRAY_BUFFER, buffers[1]); SHOW_ERROR
	glVertexAttribPointer(sh->a_texcoord, 2, GL_FLOAT, GL_FALSE, 2 * sizeof(GLfloat), NULL); SHOW_ERROR
	glEnableVertexAttribArray(sh->a_texcoord); SHOW_ERROR

	glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, buffers[2]); SHOW_ERROR

	glDrawElements(GL_TRIANGLES, kIndexCount, GL_UNSIGNED_SHORT, 0); SHOW_ERROR
}

extern int BG_PALETTE[];
#define RGB15(r, g, b)  (((r) << (5+6)) | ((g) << 6) | (b))

static void gles2_Draw(int _w, int _h, uint16_t *pixels)
{
	if(!shader.program || !shader_filtering.program)
		return;

	float sx = 1.0f, sy = 1.0f;
/*
	// Screen aspect ratio adjustment
	float a = (float)_w/_h;
	float a0 = 336.0f/240.0f;
	if(a > a0)
		sx = a0/a;
	else
		sy = a/a0;
*/
	glClear(GL_COLOR_BUFFER_BIT); SHOW_ERROR
	glViewport(0, 0, _w, _h); SHOW_ERROR

	ShaderInfo *sh = op_filtering ? &shader_filtering : &shader;

	float proj[4][4];
	glDisable(GL_BLEND); SHOW_ERROR
	glUseProgram(sh->program); SHOW_ERROR
	SetOrtho(proj, -0.5f, +0.5f, +0.5f, -0.5f, -1.0f, 1.0f, sx*op_zoom, sy*op_zoom);
	glUniformMatrix4fv(sh->u_vp_matrix, 1, GL_FALSE, &proj[0][0]); SHOW_ERROR

	if(palette_changed)
	{
		int i;
		unsigned short palette[256];
		for (i=0; i < 256; i++)
			palette[i] = BG_PALETTE[i];
		palette_changed = 0;
		glActiveTexture(GL_TEXTURE1); SHOW_ERROR
	    glBindTexture(GL_TEXTURE_2D, textures[1]); SHOW_ERROR
		glTexSubImage2D(GL_TEXTURE_2D, 0, 0, 0, 256, 1, GL_RGB, GL_UNSIGNED_SHORT_5_6_5, palette); SHOW_ERROR
	}

	glActiveTexture(GL_TEXTURE0); SHOW_ERROR
    glBindTexture(GL_TEXTURE_2D, textures[0]); SHOW_ERROR
	glTexSubImage2D(GL_TEXTURE_2D, 0, 0, 0, 1024, HEIGHT, GL_LUMINANCE, GL_UNSIGNED_BYTE, pixels); SHOW_ERROR
	//glTexImage2D(GL_TEXTURE_2D, 0, GL_LUMINANCE, TEX_WIDTH, TEX_HEIGHT, 0, GL_LUMINANCE, GL_UNSIGNED_BYTE, pixels); SHOW_ERROR
	gles2_DrawQuad(sh, textures);

	glBindBuffer(GL_ARRAY_BUFFER, 0); SHOW_ERROR
	glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, 0); SHOW_ERROR
//	glFlush();
}


void gles_exit()
{
	gles2_destroy();
	// Release OpenGL resources
	eglMakeCurrent( display, EGL_NO_SURFACE, EGL_NO_SURFACE, EGL_NO_CONTEXT );
	eglDestroySurface( display, surface );
	eglDestroyContext( display, context );
	eglTerminate( display );
}

void gles_draw(uint16_t *pixels)
{
	gles2_Draw(screen_width, screen_height, pixels);
	eglSwapBuffers(display, surface);
}