#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <GL/glut.h>
#include "../jlib/jlib.h"
#include "Vec3f.h"
#include <iostream.h>

// CONSTANTS
const int CIRCLE_SEGS = 50;
const double Pi = 3.14159265;
const bool STEPBYSTEP = false;
const double STEPSIZE = 0.005;
const double correctionStrength = 1/STEPSIZE * 1.0;

//
// VARIABLES
//

// View globals
int mode;
double beginx, beginy;
double dis = 3, azim = 0.0, elev = 0.0;
double ddis = 0.0, dazim = 0.0, delev = 0.0;

// Bead Info
Vec3f BeadX;
Vec3f BeadV;
double mass = 1;	// bead mass (kg)

double g = 9.8;		// gravity accel (m/s^2)
Vec3f FGravity(0,-g, 0);
double frictionCoeff = 0.1;



void
init_viewer_window()
{
  GLfloat Ambient[] = { 1.0f, 1.0f, 1.0f, 1.0f };  
  GLfloat Diffuse[] = { 0.2f, 0.2f, 0.2f, 1.0f };  
  GLfloat Specular[] = { 0.1f, 0.1f, 0.1f, 1.0f };   
  GLfloat SpecularExp[] = { 50 };              
  GLfloat Emission[] = { 0.1f, 0.1f, 0.1f, 1.0f };   

  glMaterialfv(GL_FRONT, GL_AMBIENT, Ambient);
  glMaterialfv(GL_FRONT, GL_DIFFUSE, Diffuse);
  glMaterialfv(GL_FRONT, GL_SPECULAR, Specular);
  glMaterialfv(GL_FRONT, GL_SHININESS, SpecularExp);
  glMaterialfv(GL_FRONT, GL_EMISSION, Emission);

  glMaterialfv(GL_BACK, GL_AMBIENT, Ambient);
  glMaterialfv(GL_BACK, GL_DIFFUSE, Diffuse);
  glMaterialfv(GL_BACK, GL_SPECULAR, Specular);
  glMaterialfv(GL_BACK, GL_SHININESS, SpecularExp);
  glMaterialfv(GL_BACK, GL_EMISSION, Emission);

  glColorMaterial(GL_FRONT_AND_BACK, GL_DIFFUSE);

  glEnable(GL_COLOR_MATERIAL);

  GLfloat light_position[] = { 1.0, 1.0, 1.0, 0.0 };
  glLightfv(GL_LIGHT0, GL_POSITION, light_position);
  glEnable(GL_LIGHT0);
  glEnable(GL_LIGHTING);
  glLightModeli(GL_LIGHT_MODEL_TWO_SIDE, GL_TRUE);

  glDepthFunc(GL_LEQUAL);
  glEnable(GL_DEPTH_TEST);
  glEnable(GL_CULL_FACE);
  glCullFace(GL_BACK);

  glShadeModel(GL_FLAT);
  glClearColor(0.0, 0.0, 0.0, 0.0);

  glMatrixMode(GL_PROJECTION);
  glLoadIdentity();
  glFrustum(-0.004,0.004,-0.004,0.004,.01,100.0);

  glMatrixMode(GL_MODELVIEW);
}

void
cb_mouse(int _b, int _s, int _x, int _y)
{
  if (_s == GLUT_UP)
  {
    dis += ddis;
    if (dis < .1) dis = .1;
    azim += dazim;
    elev += delev;
    ddis = 0.0;
    dazim = 0.0;
    delev = 0.0;
    return;
  }

  if (_b == GLUT_RIGHT_BUTTON)
  {
    mode = 0;
    beginy = _y;
    return;
  }
  else
  {
    mode = 1;
    beginx = _x;
    beginy = _y;
  }
}

void
cb_motion(int _x, int _y)
{
  if (mode == 0)
  {
    ddis = dis * (double)(_y - beginy)/200.0;
  }
  else
  {
    dazim = (_x - beginx)/5;
    delev = (_y - beginy)/5;      
  }
  
  glutPostRedisplay();
}



//
// UPDATE FUNCTION
//

Vec3f Acceleration(const Vec3f &p, const Vec3f &v)
{
	// Compute constrained acceleration.
	double lambda = (-FGravity.dot(p) - mass * v.dot(v)) / p.dot(p);
	Vec3f FHat = lambda * p;
	Vec3f FConstraint = (FGravity + FHat);

	// Compute correction force to keep it on the track.
	Vec3f OnWireX = p.norm_value();		// Nearest wire position.  Move the bead towards here.
	Vec3f FCorrect = (OnWireX - p) * correctionStrength;

	// Compute the friction force along the wire.
	Vec3f FFriction = -v * frictionCoeff;

	return (FConstraint + FCorrect + FFriction) / mass;
}

Vec3f Velocity(const Vec3f &p, const Vec3f &v)
{
	return v;
}

void DoRK3(Vec3f &p, Vec3f &v, double timestep)
{
	Vec3f k1, k2, k3;
	Vec3f l1, l2, l3;

	k1 = timestep * Velocity(p, v);
	l1 = timestep * Acceleration(p, v);
	k2 = timestep * Velocity(p + k1/2, v + l1/2);
	l2 = timestep * Acceleration(p + k1/2, v + l1/2);
	k3 = timestep * Velocity(p + k2, v + l2);
	l3 = timestep * Acceleration(p + k2, v + l2);

	p += k1/6 + k2*2/3.0 + k3/6;
	v += l1/6 + l2*2/3.0 + l3/6;
}

void DoStep()
{
	// Do one step of the movement.
	DoRK3(BeadX, BeadV, STEPSIZE);

	glutPostRedisplay();
}

void InitData()
{
	BeadX = Vec3f(cos(Pi/2+0.3),sin(Pi/2+0.3), 0);
	BeadV = Vec3f(0,0, 0);
}

void cb_keyboard(unsigned char key, int Mx, int My) 
{
	switch(key) 
	{
	case 27:	// ESC 
		exit(0);
		break;
	case 'r':	// RESET
		InitData();
		cout << "Reset!" << endl;
		break;
	case '[':
		frictionCoeff /= 2;
		cout << "Friction coeff = " << frictionCoeff << endl;
		break;
	case ']':
		frictionCoeff *= 2;
		cout << "Friction coeff = " << frictionCoeff << endl;
		break;
	default:
		DoStep();
		break;
	}

	glutPostRedisplay();
}

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

	glLoadIdentity(); 
	glTranslatef(0.0, 0.0, -(dis+ddis));
	glRotated(elev+delev, 1.0, 0.0, 0.0);
	glRotated(azim+dazim, 0.0, 1.0, 0.0);
	//glRotated(90.0,-1.0,0.0,0.0);

	// Draw the Loop.
	glColor3f(1, 1, 1);
	glBegin(GL_LINE_LOOP);
	for (int i = 0; i < CIRCLE_SEGS; ++i)
		glVertex3f(cos(2*Pi*i/double(CIRCLE_SEGS)), sin(2*Pi*i/double(CIRCLE_SEGS)), 0);
	glEnd();

	// Draw the Ball.
	glTranslatef(BeadX[0], BeadX[1], BeadX[2]);
	glColor3f(1, 0.4, 0.4);
	glutSolidSphere(0.1, 10, 10);

	glFlush();
	glutSwapBuffers();
}


void InitScene()
{
}

void main(int argc, char **argv)
{
  // init glut

  glutInit(&argc, argv);
  glutInitDisplayMode(GLUT_DOUBLE | GLUT_RGB | GLUT_DEPTH | GLUT_MULTISAMPLE);

  // create the window

  glutInitWindowSize(500, 500);
  glutInitWindowPosition(500, 100);
  glutCreateWindow("259 Hw3");

  // set OpenGL graphics state -- material props, perspective, etc.

  init_viewer_window();

  // set the callbacks

  glutDisplayFunc(cb_display);
  glutMouseFunc(cb_mouse);
  glutMotionFunc(cb_motion);  
  glutKeyboardFunc(cb_keyboard);
  if (!STEPBYSTEP)
	  glutIdleFunc(DoStep);

  // Create models
  InitScene();

  InitData();

  glutMainLoop();
}