#include <glut.h>
#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <pthread.h>
#include "nm.h"

#define     WIDTH   800
#define     HEIGHT  600

void *nm_run(void*);
void init(float,float,float,float,float,float);
void reinit(void);

int			dx,dy;
GLfloat     rx,ry,rz,drx,dry,drz;
int         r,c;
GLfloat		*x,*y,*z;
GLfloat     gp,gf,gt;

VECT3D*     path;
int         pathCap,pathLen;
GLfloat     pathZ;

pthread_t   pt;
int         pt_stop;

float       grid[6];


double f(double x, double y) {
    return
        0.2*(x*x + y*y)
        - 2*exp(-(x*x+y*y))
        - exp(-(x*x+(y+4)*(y+4))/2);
}

void reinit(void) {
    init(grid[0],grid[1],grid[2],grid[3],grid[4],grid[5]);
}

void init(float xmin, float xmax, float xstep, float ymin, float ymax, float ystep) {
	int			i,j,n;
	GLfloat		vx,vy;

    pt_stop = 0;
    grid[0] = xmin;
    grid[1] = xmax;
    grid[2] = xstep;
    grid[3] = ymin;
    grid[4] = ymax;
    grid[5] = ystep;

    gp = 1.0;
    gf = 0.001;

    rx = 0.0;
    ry = 0.0;
    rz = 0.0;
    drx = 0.0;
    dry = 0.0;
    drz = 0.0;

	dx = (int)((xmax-xmin)/xstep);
	dy = (int)((ymax-ymin)/ystep);

	x = (GLfloat*)malloc(sizeof(GLfloat)*dx*dy);
	y = (GLfloat*)malloc(sizeof(GLfloat)*dx*dy);
	z = (GLfloat*)malloc(sizeof(GLfloat)*dx*dy);

	for (i=0;i<dx;i++) {
		for (j=0;j<dy;j++) {
			n = j*dx+i;
			vx = xmin+xstep*i;
			vy = ymin+ystep*j;
			x[n] = vx;
			y[n] = vy;
			z[n] = f(vx,vy);
		}
	}

    pathCap = 100;
    pathLen = 0;
    path = (VECT3D*)malloc(sizeof(VECT3D)*pathCap);

	GLfloat lit_ambient[]	=	{.90, .90, .90, 1.0};
	GLfloat lit_diffuse[]	=	{.80, .80, .80, 1.0};
	GLfloat lit_specular[]	=	{1.0, 1.0, 1.0, 1.0};
	GLfloat lit_position[]	=	{  0,   0,   1, 1.0};

	glEnable(GL_LIGHTING);
	glLightModelf(GL_LIGHT_MODEL_TWO_SIDE,1.0);
	glLightModelfv(GL_LIGHT_MODEL_AMBIENT,lit_ambient);
	glLightfv(GL_LIGHT0,GL_AMBIENT,lit_ambient);
	glLightfv(GL_LIGHT0,GL_DIFFUSE,lit_diffuse);
	glLightfv(GL_LIGHT0,GL_SPECULAR,lit_specular);
	glLightfv(GL_LIGHT0,GL_POSITION,lit_position);
	glEnable(GL_LIGHT0);

	glEnable(GL_DEPTH_TEST);
	glEnable(GL_NORMALIZE);

    glEnable(GL_SMOOTH);
    glEnable(GL_LINE_SMOOTH);
    glEnable(GL_BLEND);
    glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);

	glClearColor(1.0, 1.0, 1.0, 1.0);
    pthread_create(&pt,NULL,nm_run,NULL); 
}

void normal(GLfloat x1, GLfloat y1, GLfloat z1,
			GLfloat x2, GLfloat y2, GLfloat z2,
			GLfloat x3, GLfloat y3, GLfloat z3,
			GLfloat *xn, GLfloat *yn, GLfloat *zn) {
   *xn = (y2-y1)*(z3-z1) - (z2-z1)*(y3-y1);
   *yn = (z2-z1)*(x3-x1) - (x2-x1)*(z3-z1);
   *zn = (x2-x1)*(y3-y1) - (y2-y1)*(x3-x1);
}

void displayFunc(void) {

	int i,j,n;

	glPolygonMode(GL_FRONT_AND_BACK,GL_LINE);
    glLineWidth(1.0);

	GLfloat	mat_ambient[]	=	{.25, .22, .06, 1.0};
	GLfloat mat_diffuse[]	=	{.35, .31, .09, 1.0};
	GLfloat mat_specular[]	=	{.80, .72, .21, 1.0};

	glMaterialfv(GL_FRONT_AND_BACK,GL_AMBIENT,mat_ambient);
	glMaterialfv(GL_FRONT_AND_BACK,GL_DIFFUSE,mat_diffuse);
	glMaterialfv(GL_FRONT_AND_BACK,GL_SPECULAR,mat_specular);
	glMaterialf(GL_FRONT_AND_BACK,GL_SHININESS, 95.0);

	glBegin(GL_QUADS);
	for (i=1;i<dx;i++) {
		for (j=1;j<dy;j++) {
			n = j*dx+i;
			glVertex3f(x[n-1],y[n-1],z[n-1]);
			glVertex3f(x[n-dx-1],y[n-dx-1],z[n-dx-1]);
			glVertex3f(x[n-dx],y[n-dx],z[n-dx]);
			glVertex3f(x[n],y[n],z[n]);
		}
	}
	glEnd();
}

void displayNM(void) {
    int i,l;
	glPolygonMode(GL_FRONT_AND_BACK,GL_FILL);

	GLfloat	mat_ambient[]	=	{0.0, .06, .22, 0.5};
	GLfloat mat_diffuse[]	=	{0.0, .09, .31, 0.5};
	GLfloat mat_specular[]	=	{0.0, .21, .72, 0.5};

	glMaterialfv(GL_FRONT_AND_BACK,GL_AMBIENT,mat_ambient);
	glMaterialfv(GL_FRONT_AND_BACK,GL_DIFFUSE,mat_diffuse);
	glMaterialfv(GL_FRONT_AND_BACK,GL_SPECULAR,mat_specular);
	glMaterialf(GL_FRONT_AND_BACK,GL_SHININESS, 95.0);
    glBegin(GL_TRIANGLES);
        for (i=0,l=pathLen;i<l;i++)
            glVertex3f(path[i].x,path[i].y,path[i].z);
    glEnd();

    glPolygonMode(GL_FRONT_AND_BACK,GL_LINE);
    glLineWidth(2.0);
    glDisable(GL_LIGHTING);
    glColor3f(0.,0.,0.);
    glBegin(GL_TRIANGLE_STRIP);
        for (i=0,l=pathLen;i<l;i++)
            glVertex3f(path[i].x,path[i].y,path[i].z);
    glEnd();
    glEnable(GL_LIGHTING);
}

void display(void) {
	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

    //setup the camera
    glMatrixMode(GL_PROJECTION);
    glLoadIdentity();
    gluPerspective(15.0, 3.0/4.0, .5, 200.0);
    gluLookAt(100.0-70.0*(1.0-gp), 100.0-70.0*(1.0-gp),-10.0 + 90.0*gp,0.0,0.0,-5.0,0.0,0.0,1.0);
    glTranslatef(-10.0*gp,-10*gp,-30.0+22.0*(1.0-gp));
    glMatrixMode(GL_MODELVIEW);

    glPushMatrix();
	glRotatef(rx, 1.0, 0.0, 0.0);
    glRotatef(ry, 0.0, 1.0, 0.0);
    glRotatef(rz, 0.0, 0.0, 1.0);

    displayFunc();
    displayNM();

	glPopMatrix();

	glutSwapBuffers();
    glutPostRedisplay();
}

void nm_callback(VECT3D* v) {
    int         i;
    pthread_t   mt;
    GLfloat     zm = v[0].z;

    mt = pthread_self();
    if (!pthread_equal(mt,pt)) {
        pthread_exit(NULL);
    }

    if ((pathLen+3) < pathCap) {
        for (i=0;i<3;i++) {
            path[pathLen+i].x = v[i].x;
            path[pathLen+i].y = v[i].y;
            path[pathLen+i].z = v[i].z;
            if (v[i].z < zm)
                zm = v[i].z;
        }
        //TODO: mutex lock
        pathLen += 3;
        pathZ = zm;
        //TODO: mutex unlock
    }
    else {
        fprintf(stderr,"out of space\n");
        //TODO: Allocate more space & copy.
    }

    usleep(1000000);
}

void *nm_run(void* arg) {
    nm(f,x[0],x[1],1.0e-5,nm_callback);
    return NULL;
}

void idle(void) {

    gt = (pathZ+3.0)/43.0;
    gp -= (gp-gt)*gf;

    if (r) {
        rx += drx;
        ry += dry;
        rz += drz;
        if (rx > 360)
            rx -= 360;
        if (ry > 360)
            ry -= 360;
        if (rz > 360)
            rz -= 360;
    }
}

void motion(int x, int y) {
    if (r) {
        x = (WIDTH>>1) - x;
        y = (HEIGHT>>1) - y;

        drz = 0.5*(float)x/(HEIGHT>>1);
    }
}

void mouse(int button, int state, int x, int y) {
    if (button == GLUT_LEFT_BUTTON) {
        r = !state;
    }
    else if ((button == GLUT_RIGHT_BUTTON)&&state) {
        reinit();
    }
}

void safeexit(void) {
    free(x);
    free(y);
    free(z);

    if (path != NULL)
        free(path);
}

int main(int argc, char** argv) {

    atexit(safeexit);

	glutInit(&argc,argv);
	glutInitDisplayMode(GLUT_DOUBLE | GLUT_RGB | GLUT_DEPTH);
	glutInitWindowSize(WIDTH,HEIGHT);
	glutCreateWindow("Nelder-Mead");
	glutDisplayFunc(display);
	glutMouseFunc(mouse);
    glutMotionFunc(motion);
	glutIdleFunc(idle);
	init(-10.,10.,0.5,-10.,10.,0.5);
	glutMainLoop();
	return 0;
}