/**
  Reads robotics range dataset.
  
  Orion Sky Lawlor, olawlor@acm.org, 2008-08-23 (Public Domain)
*/
#include <GL/glew.h> /*<- for gl...ARB extentions.  Must call glewInit after glutCreateWindow! */
#include <iostream>
#include <GL/glut.h>
#include <math.h>
#include "osl/vec4.h" /*<- handy little vector class */
#include "osl/mat4.h"
using osl::mat4;
/* Convert a struct with .x, .y, .z fields into three arguments */
#define VEC3_TO_XYZ(v) (v).x,(v).y,(v).z


#include "ogl/image.h" /* (originally from) Nigel Stewart's GLT image library */
#include "ogl/image.cpp"

#include "ogl/glsl.h" /* Orion's tiny GLSL library */
#include "ogl/glsl.cpp"

#include "GL/glui.h" /* OpenGL User Interface library (for making a GUI) */
#ifndef M_PI
#define M_PI 3.14159265358979323
#endif

void oglExit(const char *what,const char *why) {
	printf(what,why);
	exit(1);
}

// Check GL for errors after the last command
void oglCheck(const char *where) {
        GLenum e=glGetError();
        if (e==GL_NO_ERROR) return;
        const GLubyte *errString = gluErrorString(e);
        oglExit(where, (const char *)errString);
}

float walking_speed=3.0;
float camera_height=1.7; // meters above ground for observer
float robot_height=0.30; // meters above ground for robot sensor origin
float robot_vbias=0.27; // radians above horizon for robot sensor center
float point_size=0.03; // display size of points (meters)

/********************* File I/O datatypes **********************/
typedef unsigned char io_byte;

class Lil32 { //Little-endian (Intel byte order) 32-bit integer
	io_byte d[4];
public:
	Lil32() {}
	Lil32(unsigned int i) { set(i); }
	operator unsigned int () const { return d[0]|(d[1]<<8)|(d[2]<<16)|(d[3]<<24); }
	unsigned int operator=(unsigned int i) {set(i);return i;}
	void set(unsigned int i) { 
		d[0]=(io_byte)i; 
		d[1]=(io_byte)(i>>8); 
		d[2]=(io_byte)(i>>16); 
		d[3]=(io_byte)(i>>24); 
	}
};

class LilFloat32 { //Little-endian (Intel byte order) 32-bit float
	Lil32 i;
public:
	operator float () const { 
		union {int i;float f;} conv;
		conv.i=i;
		return conv.f;
	}
	float operator=(float f) {
		union {int i;float f;} conv;
		conv.f=f;
		i.set(conv.i);
		return f;
	}
};
/******************* Object, read from .obj file ***************/
class range_mesh {
public:
	range_mesh(const char *fName);
	
	void draw(const vec3 &cameraLoc);
	
	// GUI stuff (probably doesn't belong right here)
	int enabled; /* display this mesh? */
	vec3 origin; /* our coordinate system origin */
	float rotation; /* XY axis rotation to apply */
	mat4 toWorld; /* image transformation */
private:
	struct myVertex {
		vec3 xyz; /* location */
		myVertex(const vec3 &v) :xyz(v) {}
	};
	std::vector<myVertex> vtx; /* 3D location of vertices (0-based) */
	
	struct myFace {
		int v[3]; /* 1-based vertex index */
		int n[3]; /* 1-based normal vector index */
	};
	std::vector<myFace> tri;
};

void print(const vec3 &v) {
	printf("	%.5f,%.5f,%.5f\n",v.x,v.y,v.z);
}

range_mesh::range_mesh(const char *fName)
	:enabled(true), origin(0.0), rotation(0.0), toWorld(1.0)
{
	FILE *f=fopen(fName,"rb");
	
struct range_file_header {
	Lil32 A[2]; /* some sort of file header */
	
	char k1;
	Lil32 w; /* width of image, in columns */
	
	char k2;
	Lil32 h; /* height of image, in rows */
	
	char k3; 
	LilFloat32 posx; /* location of sensor, X axis (meters) */
	char k4;
	LilFloat32 posy; /* location of sensor, Y axis (meters) */
	
	char k5; 
	LilFloat32 axisx; /* direction of sensor, X axis */
	char k6;
	LilFloat32 axisy; /* direction of sensor, Y axis */
	
	char k7; 
	LilFloat32 hfov; /* field of view, degrees, along X axis */
	char k8;
	LilFloat32 vfov; /* field of view, degrees, along Y axis */
	
	char k9;
	Lil32 time1,time2; /* seconds? */
	char kA;
	Lil32 seq; /* image sequence number */
	
	char kB;
	LilFloat32 maxrange; /* maximum distance range sensor can read */
	char kC;
	LilFloat32 vbias; /* sensor direction bias, along y axis (radians?) */
	char kD;
	LilFloat32 hbias; /* sensor direction bias, along x axis (radians?) */
};
	enum {assert_header_size=1/((sizeof(range_file_header)==77)?1:0)};
	range_file_header params;
	fread(&params,1,sizeof(params),f);
	int w=params.w, h=params.h;
	float mR = 0; // Mean radius
	float mT = 0; // Mean theta
	
	origin.x=-params.posx; origin.y=-params.posy;
	rotation=-acos(-params.axisx); // -params.axisx; /* funky-- only seems to have one nonzero value; maybe it's cos(angle)? */
	
	printf("File %s: location %.3f,%.3f meters; orientation %.3f,%.3f cos-rads; FOV %.3f,%.3f degrees; bias %.3f rads; max %.3f meters\n",
		fName,(float)params.posx,(float)params.posy,
		(float)params.axisx,(float)params.axisy,
		(float)params.hfov,(float)params.vfov,
		(float)params.vbias,
		(float)params.maxrange);
	
	float vbias=params.vbias*M_PI/180.0; 
	// vbias=robot_vbias; // better than params.vbias for 07-08 images...

	/* Read ranges from file, and project into 3D vertices */
	for (int fr=0;fr<h;fr++)
	{
		for (int fc=0;fc<w;fc++) 
		{
			LilFloat32 ra_file;
			if (fread(&ra_file,sizeof(ra_file),1,f)!=1) oglExit("Error reading range data from file %s\n",fName);
			float ra=ra_file; // range to point
			if (ra>=params.maxrange) continue; /* skip that point */
			
			int r=fr; /* Do images need to be flipped coming out of the file? */
			int c=fc; 
			float deg2rad=M_PI/180.0;
			
			float th = ((r - 0.5*h)*(1.0/h))*params.vfov*deg2rad+vbias; // angle theta, rads
			float ph = ((0.5*w - c)*(1.0/w))*params.hfov*deg2rad; // angle phi, rads
			vtx.push_back(myVertex(vec3(
				ra*sin(ph), // x (shift)
				(ra*cos(th-mT)+mR*cos(th))*cos(ph), // y (distance)
				(ra*sin(th-mT)+mR*sin(th))*cos(ph) // z (height)
			)));
		}
	}
	print(vtx[0].xyz);
	print(vtx[1000].xyz);
	print(vtx[vtx.size()/2].xyz);
	print(vtx[vtx.size()-1].xyz);
	fclose(f);
}

#include "nice_points.h"
void range_mesh::draw(const vec3 &cameraLoc)
{
	if (!enabled) return;
	bool need_color = nice_points_begin(point_size,cameraLoc);
	glBegin(GL_POINTS);
	for (unsigned int v=0;v<vtx.size();v++) {
		if (need_color) glColor3fv(vtx[v].xyz);
		glVertex3fv(vtx[v].xyz);
	}
	glEnd();
	nice_points_end();
}
/*********************** App Code **********************/

/* Orthonormal coordinate frame */
class ortho_frame {
public:
	/* Unit vectors pointing along axes of our frame.
		X cross Y is Z. (right-handed coordinate system)
	*/
	vec3 x,y,z;
	ortho_frame() :x(1,0,0), y(0,1,0), z(0,0,1) {}
	
	/* Reorient this coordinate frame by this far 
	   along in the X and Y axes.  "dx" is the distance
	   along the z axis to push the x axis; "dy" 
	   the distance along the z axis to push the y axis.
	*/
	void nudge(double dx,double dy) {
		x+=dx*z;
		y+=dy*z;
		orthonormalize();
	}
	
	/* Reconstruct orthonormal frame from X and Y axes.
	  Y is primary, X is secondary, Z is tertiary.
	*/
	void orthonormalize(void) {
		y=normalize(y);
		z=normalize(cross(x,y));
		x=normalize(cross(y,z));
	}
};

/* Orientation of our camera. Camera looks down +Z axis; Y points down. */
ortho_frame camera_frame;

/* Location of our camera */
vec3 camera_location=vec3(0,-3,1.7);

/* Corresponding key is down */
bool key_is_down[256]={false};

/* Time it took to draw the last frame (for updating physics) */
float seconds_per_frame=0.01;

/* Nice handy timer: returns seconds */
double nice_timer(void) {return glutGet(GLUT_ELAPSED_TIME)*1.0e-3;}

/**
 Stores an entire OpenGL scene.
*/
class the_world {
public:
	the_world(GLUI *glui,char **range_files);
	
	/* Precompute values needed for a given frame.  May trash matrices! */
	void setup(void);
	
	/* Render world to the current modelview and projection matrices */
	void render(bool for_camera);
private:
	GLuint planeTex; /* used by world plane */
	
	mat4 all_range; /* global transformation for range models */
	std::vector<range_mesh *> range; /* our range models */
};

the_world::the_world(GLUI *glui,char **range_files)
	:all_range(1.0)
{
	new GLUI_Column(glui); /* for layout */
	new GLUI_StaticText(glui,"Display:");
	
	for (int argi=0;range_files[argi]!=0;argi++)
	{ /* Read one range image */
		range_mesh *m=new range_mesh(range_files[argi]);
		range.push_back(m);
		char label[100];
		sprintf(label,"Range %d",argi);
		new GLUI_Checkbox(glui,label,&m->enabled);
	}
	
	new GLUI_Column(glui); /* for layout */
	new GLUI_Rotation(glui,"Overall",&all_range[0][0]);
	
	// Make per-image GUI control objects
	for (unsigned int r=0;r<range.size();r++)
	{
		range_mesh *m=range[r];
		new GLUI_Column(glui); /* for layout */
		char label[100];
		sprintf(label,"Range %d",r);
		new GLUI_Rotation(glui,label,&m->toWorld[0][0]);
		new GLUI_Spinner(glui, "X: ", &m->origin.x);
		new GLUI_Spinner(glui, "Y: ", &m->origin.y);
		new GLUI_Spinner(glui, "Rot: ", &m->rotation);
	}

/* Make textured plane */
	oglCheck("Before texture parameter setting");
	glGenTextures(1,&planeTex); /* make texture handle */
	glBindTexture(GL_TEXTURE_2D,planeTex);
	readTextureFromFile("plane.bmp");
	glTexParameteri(GL_TEXTURE_2D,GL_TEXTURE_WRAP_S,GL_REPEAT);
	glTexParameteri(GL_TEXTURE_2D,GL_TEXTURE_WRAP_T,GL_REPEAT);
	glTexParameteri(GL_TEXTURE_2D,GL_TEXTURE_MIN_FILTER,
		GL_LINEAR_MIPMAP_LINEAR);
	glTexParameteri(GL_TEXTURE_2D,GL_TEXTURE_MAG_FILTER,GL_LINEAR);
	glTexParameteri(GL_TEXTURE_2D,GL_TEXTURE_MAX_ANISOTROPY_EXT,16);
	glTexParameteri(GL_TEXTURE_2D,GL_GENERATE_MIPMAP_SGIS, GL_TRUE);	
	glGetError(); // Ignore errors from setting good texture state...
}

/* Render the world.  Assumes matrices are already set up. */
void the_world::render(bool from_camera) {
/* Clear the sky */
	glClear(GL_COLOR_BUFFER_BIT + GL_DEPTH_BUFFER_BIT);
	
	oglCheck("Before main render loop");
	/* Draw the giant ground plane */
	glColor3f(1,1,1);
	glBindTexture(GL_TEXTURE_2D,planeTex);
	glEnable(GL_TEXTURE_2D);
	glBegin (GL_TRIANGLE_FAN);
	float z=0, s=1.0e3;
	float t=s; /* texture and world coordinates are equal -> one meter per texture repeat */
	glTexCoord2f(-t,-t);
        glVertex3f(-s,-s,z); 
	glTexCoord2f(+t,-t);
        glVertex3f(+s,-s,z); 
	glTexCoord2f(+t,+t);
        glVertex3f(+s,+s,z); 
	glTexCoord2f(-t,+t);
        glVertex3f(-s,+s,z); 
        glEnd();
	glDisable(GL_TEXTURE_2D);
	
	/* Draw the range meshes */
	for (unsigned int r=0;r<range.size();r++)
	{
		glPushMatrix(); 
		range[r]->origin.z=robot_height;
		glTranslatef(VEC3_TO_XYZ(range[r]->origin));
		glRotatef(range[r]->rotation*180/M_PI, 0,0,1);
		glMultMatrixf(&range[r]->toWorld[0][0]);
		glMultMatrixf(&all_range[0][0]);
		range[r]->draw(camera_location);
		glPopMatrix(); 
	}
	oglCheck("After main render loop");
}

the_world *world=0; /* world model (can't initialize it until OpenGL is set up!) */

void my_display(void) {
	glFinish();
	int last_msec=glutGet(GLUT_ELAPSED_TIME);
	
	glClearColor(0.3,0.5,0.7,0); /* set background color to sky blue */
	
/* Reset matrices */
	glMatrixMode(GL_MODELVIEW); /* set up model and view matrix */
	glLoadIdentity(); /* reset matrix at start of frame */
		
	glMatrixMode(GL_PROJECTION); /* set up projection matrix */
	glLoadIdentity(); /* reset matrix at start of frame */
	
	
/* Move the camera */
	float dt=seconds_per_frame;/* time between physics updates, seconds */
	
	float vel=walking_speed; /* velocity, in m/s */
	float forward_dir=0.0, strafe_dir=0.0;
	if (key_is_down[(int)'w']) {forward_dir+=1.0;}
	if (key_is_down[(int)'s']) {forward_dir-=1.0;}
	if (key_is_down[(int)'a']) {strafe_dir-=1.0;}
	if (key_is_down[(int)'d']) {strafe_dir+=1.0;}
	
	vec3 camera_vel = vel*dt*(
		camera_frame.z*forward_dir+
		camera_frame.x*strafe_dir
	);
	camera_location += camera_vel;

	if (length(camera_vel)>0.01f) glutPostRedisplay();
	
	/* You are walking: */
	camera_location.z=camera_height; /* you can't fly */
	
	camera_frame.x.z=0.0; /* you can't tilt your head */
	camera_frame.y=cross(camera_frame.z,camera_frame.x);
	
	camera_frame.orthonormalize();
	
/* Set up camera */
	gluPerspective(90.0, /* <- Y camera field-of-view, in degrees */
		glutGet(GLUT_WINDOW_WIDTH)/(float)glutGet(GLUT_WINDOW_HEIGHT), /* aspect ratio: width over height */
		0.1, /* Near clipping plane depth */
		100.0 /* Far clipping plane depth */
	);
	gluLookAt(VEC3_TO_XYZ(camera_location),
		VEC3_TO_XYZ(camera_location + camera_frame.z), /* look down +Z */
		VEC3_TO_XYZ(-camera_frame.y) /* OpenGL +Y points up, our Y points down */
	);
	
	glMatrixMode(GL_MODELVIEW); /* set up model and view matrix */
	glLoadIdentity(); /* reset matrix at start of frame */
	
	glClear(GL_COLOR_BUFFER_BIT + GL_DEPTH_BUFFER_BIT);
	glEnable(GL_DEPTH_TEST);
	

/* Draw the world */
	world->render(true);
	
	glutSwapBuffers(); /* copy back buffer to front */
	
/* Statistics */
	glFinish();
	int cur_msec=glutGet(GLUT_ELAPSED_TIME); /* milliseconds since glutInit */
	int msec_elapsed=cur_msec-last_msec;
	printf("%d msec/frame; %.2f fps\n",
		msec_elapsed,1.0e3/msec_elapsed);
	last_msec=cur_msec;
	seconds_per_frame=msec_elapsed*1.0e-3;
}

int mouse_x, mouse_y;
void my_mouse(int state,int buttons,int x,int y) {
	mouse_x=x; mouse_y=y;  /* just stash motion */
}
void my_motion(int x,int y) { 
	float dx=x-mouse_x, dy=y-mouse_y; /* relative motion */
	
	float speed=1.0/100; /* radians rotation per pixel */
	camera_frame.nudge(speed*dx,speed*dy);
	
	mouse_x=x; mouse_y=y; 
	glutPostRedisplay();
}

void my_keyboard(unsigned char key,int mouse_x,int mouse_y) 
{
	//printf("Key '%c' going down\n",key);
	key_is_down[key]=true;
	glutPostRedisplay();
}
void my_keyboardup(unsigned char key,int mouse_x,int mouse_y) 
{
	//printf("Key '%c' going back up\n",key);
	key_is_down[key]=false;
}
int main(int argc,char *argv[]) {
	/* Start camera in reasonable orientation */
	camera_frame.y=vec3(0,0,-1); camera_frame.z=vec3(0,1,0);
	camera_frame.orthonormalize();
	
	if (argc==1) {
		char *default_args[]={"main",
			"7-9-08-groundtruth/0.RangeImage",
			"7-9-08-groundtruth/1.RangeImage",
			"7-9-08-groundtruth/2.RangeImage",
			"7-9-08-groundtruth/3.RangeImage",
			"7-9-08-groundtruth/4.RangeImage",
			0
		};
		argv=default_args;
	}

	glutInitWindowSize(800,600);
	glutInit(&argc,argv);
	int mode=GLUT_DOUBLE | GLUT_RGBA | GLUT_DEPTH;
        glutInitDisplayMode (mode);
	int win=glutCreateWindow("OpenGL Point Cloud Alignment (try the WASD keys)");
	glewInit();
	
	GLUI *glui = GLUI_Master.create_glui_subwindow(win, GLUI_SUBWINDOW_TOP);
	glui->set_main_gfx_window( win );
	
	new GLUI_Spinner(glui, "Camera Z: ", &camera_height);
	new GLUI_Spinner(glui, "Robot Z: ", &robot_height);
	new GLUI_Spinner(glui, "vbias: ", &robot_vbias);
	(new GLUI_Spinner(glui, "Pointsize:",&point_size))
		-> set_float_limits(0.001,0.1);
	
	// Initialize the world
	world=new the_world(glui,&argv[1]);
	
	glutDisplayFunc(my_display);
	glutMotionFunc(my_motion);
	glutMouseFunc(my_mouse);
	glutKeyboardFunc(my_keyboard);
	glutKeyboardUpFunc(my_keyboardup);
	
	glutMainLoop(); /* processes events */
	return 0;
}
