/* 
   Project: coevolutionary GP moto driving
   Dana Vrajitoru
   gl_interface.cc
   
   Some functions to manage the interaction with the OpenGL window.
*/

#include <stdlib.h>
#include <GL/glut.h>
#include <iostream>

#include "gl_interface.h"
#include "moto_geometry.h"
#include "scenery.h"
#include "manager.h"

Manager mdr; // moto driver road
static GLfloat theta[] = {0.0,90.0,0.0};
static GLint axis = 2;
static int direction = 0;
static GLfloat trans[] = {0.0,0.0,1};
extern int pwindow, swindow;

static GLfloat theta_sml[] = {20, 50, 60};
static GLfloat trans_sml[] = {+1.9, -0.01, 0.01};
int set_flag = 0;
void render()
{
  glutSetWindow(pwindow);
  display();
  glutSetWindow(swindow);
  display_sml();
  glutSetWindow(pwindow);
}

// OpenGL function that is called to redisplay the small overview
// window.
void display_sml()
{
  static Point3f viewer;
  /* display callback, clear frame buffer and z buffer,
     rotate object and draw, swap buffers */

  glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
  glLoadIdentity();
  glClearColor(0.3, 0.3, 0.3, 1);
  glColor3f(1.0, 0.0, 0.0);
  gluLookAt(0, 0, 3.5, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0);

  glTranslatef(trans_sml[0], trans_sml[1], 0);
  glScalef(trans_sml[2], trans_sml[2], trans_sml[2]);
  glRotatef(theta_sml[0], 1.0, 0.0, 0.0);
  glRotatef(theta_sml[1], 0.0, 1.0, 0.0);
  glRotatef(theta_sml[2], 0.0, 0.0, 1.0);

  //scenery_display();
  viewer = mdr.moto->get_road_pos();
  //viewer *= -1;
  glColor3f(1.0, 0.0, 0);
  gl_sphere(viewer[0], viewer[1], viewer[2], 10, 2);
  mdr.road->display();
  glutSwapBuffers();
}

// Main OpenGL function that is called whenever an event occurs.
void display()
{
  static Point3f viewer;
  /* display callback, clear frame buffer and z buffer,
     rotate object and draw, swap buffers */

  glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
  light_init();
  glLoadIdentity();

  //set the background to sky blue
  // glClearColor(0.5294, 0.8078, 0.980, 0.2);
    glClearColor(0.680, 0.93, 0.93, 0.2);
   //glClearColor(0.2, 0.2, 0.2, 0.2);
  glColor3f(1.0, 0.0, 0.0);
  gluLookAt(0, 0, 3.5, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0);

  glTranslatef(trans[0], trans[1], 0);
  glScalef(trans[2], trans[2], trans[2]);
  glRotatef(theta[0], 1.0, 0.0, 0.0);
  glRotatef(theta[1], 0.0, 1.0, 0.0);
  glRotatef(theta[2], 0.0, 0.0, 1.0);

  //scenery_display();
  viewer = mdr.moto->get_road_pos();
  viewer *= -1;
  glPushMatrix();
  glRotatef(-mdr.moto->get_orientation_angle(), 0.0, 1.0, 0.0);
  viewer.gl_translate();
  mdr.display();
  glPopMatrix();
  glutSwapBuffers();
}

// Moves all of the objects that are animated in the scene.
void move_objects()
{
  mdr.move();
  /*driver->remove_position(mdr.moto->get_road_pos());
    mdr.driver->add_rotation(0.0, -mdr.moto->get_orientation_angle(), 0.0);
    mdr.moto->move();
    mdr.driver->add_position(mdr.moto->get_road_pos());
    mdr.driver->add_rotation(0.0, mdr.moto->get_orientation_angle(), 0.0);
    //mdr.moto->handlebar_loop();
  */
  glutPostRedisplay();
}

// Spins the entire scene by 0.2 degrees about selected axis.
void spin_objects()
{
  if (direction == 0) {
    theta[axis] += 1;
    if( theta[axis] > 360.0 ) theta[axis] -= 360.0;
  }
  else {
    theta[axis] -= 1;
    if( theta[axis] <= 0 ) theta[axis] += 360.0;
  }
  move_objects();
}

// Mouse callback, selects an axis about which to rotate. When the
// appropriate mouse button is pushed, it starts to spin the scene and
// when the mouse button is released, the spinning stops.
void mouse(int btn, int state, int x, int y)
{
  if (state != GLUT_DOWN)
    glutIdleFunc(move_objects);
  else {
    int key_state = glutGetModifiers();
    if (key_state == GLUT_ACTIVE_CTRL) {
      if (btn == GLUT_LEFT_BUTTON) {
	//if (mdr.pilot)
	//mdr.pilot->lock();
	mdr.moto->add_orientation_angle(10);
	//if (mdr.pilot)
	//mdr.pilot->unlock();
      }
    }
    else {
      glutIdleFunc(spin_objects);
      switch (btn) {
      case GLUT_LEFT_BUTTON:
	axis = 0;
	break;
      case GLUT_MIDDLE_BUTTON:
	axis = 1;
	break;
      case GLUT_RIGHT_BUTTON:
	axis = 2;
      }
      
      if (key_state == GLUT_ACTIVE_SHIFT)
	direction = 1;
      else
	direction = 0;
    }
  }
}

// A good view of the motorcycle.
void default_view()
{
  theta[0] = 2.40001;
  theta[1] = 71.4001;
  theta[2] = 0;
  trans[0] = 0;
  trans[1] = -1.2;
  trans[2] = 0.45;
}

// A view from above
void overview()
{
  theta[0] = 11.4;
  theta[1] = 71.4001;
  theta[2] = 0;
  trans[0] = 0.8;
  trans[1] = -1.2;
  trans[2] = 0.15;
}

// Callback for ASCII keyboard characters.
void skey(unsigned char key, int x, int y)
{
  int key_state = glutGetModifiers();
  if (key_state == GLUT_ACTIVE_SHIFT) {
    if (key == ' ') {
      if (mdr.moto->get_throttle_twist() < 0.15) 
	mdr.moto->add_front_break(1);
      else 
	mdr.moto->add_front_break(5);
      mdr.moto->set_throttle_twist(0); 
      return;
    }
  }
  else if (key_state == GLUT_ACTIVE_CTRL){
    if (key == ' ') {
      if (mdr.moto->get_throttle_twist() < 0.15) 
	mdr.moto->add_back_break(1);
      else 
	mdr.moto->add_back_break(5);
      mdr.moto->set_throttle_twist(0); 
      return;
    }
  }
  switch (key) {
  case 'v':
    cout << "angles: " << theta[0] << ' ' << theta[1] << ' ' << theta[2]
	 << " trans: " << trans[0] << ' ' << trans[1] << ' ' << trans[2]
	 << endl;
    break;
  case 'd':
    default_view();
    break;
  case 's':
    theta[0] = 2.40001;
    theta[1] = 71.4001;
    theta[2] = 20;
    trans[0] = 0;
    trans[1] = -1.2;
    trans[2] = 0.45;
    break;
  case 'e':
    overview();
    break;
  case 'r':
    mdr.moto->reset();
    break; 
  case 'y':
    mdr.moto->reset();
    mdr.moto->set_orientation_angle(180);
    break; 
  case 'u':
    mdr.moto->getup();
    break;
  case 'q':
  case 'Q':
    exit(0);
    break;
  case 'b':
    theta[0] = 25.4;
    theta[1] = 51.4001;
    theta[2] = 0;
    //if (mdr.pilot)
      //mdr.pilot->lock();
    mdr.moto->set_road_pos(18.7092, 8.17806e-7, -818.659);
    //if (mdr.pilot)
      //mdr.pilot->unlock();
    break; 
  case 'p':
    mdr.toggle_pilot();
    //if (mdr.pilot)
      //mdr.pilot->lock();
    //cout << mdr.moto->get_road_pos() << endl;
    // mdr.road->get_slope(mdr.moto->get_road_pos(),
    //			mdr.moto->get_orientation_angle());
    //if (mdr.pilot)
      //mdr.pilot->unlock();
    break;

  case 'n':
     set_flag = 1;
     mdr.toggle_pilot1(set_flag);
     break;
  
  case 't':
    mdr.toggle_timer();
    break;
  case ' ':
    //test_mdr.driver();
    //if (mdr.pilot)
      //mdr.pilot->lock();
    if (mdr.moto->get_throttle_twist() < 0.15) {
      mdr.moto->add_front_break(1);
      mdr.moto->add_back_break(1);
    }
    else {
      mdr.moto->add_front_break(5);
      mdr.moto->add_back_break(5);
    }
    mdr.moto->set_throttle_twist(0);
    //if (mdr.pilot)
      //mdr.pilot->unlock();
    //mdr.moto->status();
    break;
  }
  move_objects();
  display();
}

// Callback for the keyboard. The arrow keys move the entire scene on
// x and y. The page up and page down keys move the scene on z.
void key(int key, int x, int y)
{
  int key_state = glutGetModifiers();
  if (key_state == GLUT_ACTIVE_SHIFT) {
    switch (key) {
    case GLUT_KEY_LEFT:
      trans[0] -= 0.1;
      break;
    case GLUT_KEY_RIGHT:
      trans[0] += 0.1;
      break;
    case GLUT_KEY_UP:
      trans[1] += 0.1;
      break;
    case GLUT_KEY_DOWN:
      trans[1] -= 0.1;
      break;
    case GLUT_KEY_PAGE_UP:
      trans[2] += 0.05;
      break;
    case GLUT_KEY_PAGE_DOWN:
      trans[2] -= 0.05;
      break;
    }
  }
  // added by pk 
  // to set up a constant speed in case of
  // bike to come back to track again
  else if(key_state == GLUT_ACTIVE_CTRL){
    switch (key) {
     
    case GLUT_KEY_UP:
      mdr.moto->set_const_speed_forward();
      break;
    case GLUT_KEY_DOWN:
      mdr.moto->set_const_speed_backward();
      break;
    }
  }
  //------------------------------------
  else {    
    //if (mdr.pilot)
      //mdr.pilot->lock();
    switch (key) {
    case GLUT_KEY_LEFT:
      if (mdr.moto->get_speed() < 0.5)
	mdr.moto->add_left_push();
      else
	mdr.moto->lean_left();
      break;
    case GLUT_KEY_RIGHT:
      if (mdr.moto->get_speed() < 0.5)
	mdr.moto->add_right_push();
      else
	mdr.moto->lean_right();
      break;
    case GLUT_KEY_UP:
      if (mdr.moto->get_throttle_twist() < 0.15) 
	mdr.moto->add_throttle_twist(0.025);
      else
	mdr.moto->add_throttle_twist(0.01);
      break;
    case GLUT_KEY_DOWN:
      if (mdr.moto->get_throttle_twist() < 0.15) 
	mdr.moto->add_throttle_twist(-0.025);
      else
	mdr.moto->add_throttle_twist(-0.01);
      break;
    case GLUT_KEY_PAGE_UP:
      trans[2] += 0.05;
      break;
    case GLUT_KEY_PAGE_DOWN:
      trans[2] -= 0.05;
      break;
    case GLUT_KEY_HOME:
      mdr.moto->add_throttle_twist(0.0001);
      break;
    case GLUT_KEY_END:
      mdr.moto->add_throttle_twist(-0.0001);
      break;
    case GLUT_KEY_INSERT:
      //test_mdr.driver();
      mdr.status();
      break;
    }
    //if (mdr.pilot)
      //mdr.pilot->unlock();
  }
  move_objects();
  display();
}

// To be called whenever the window is resized.
void my_reshape(int w, int h)
{
  glViewport(0, 0, w, h);
  glMatrixMode(GL_PROJECTION);
  glLoadIdentity();
  gluPerspective(60.0, w/h, 0.5, 50.0); 
  /*  if (w<=h) 
    glFrustum(-10.0, 10.0, -10.0 * (GLfloat) h/ (GLfloat) w, 
              10.0* (GLfloat) h / (GLfloat) w, -10.0, 20.0);
  else 
    glFrustum(-10.0, 10.0, -10.0 * (GLfloat) w/ (GLfloat) h, 
              10.0* (GLfloat) w / (GLfloat) h, -10.0, 20.0);
  */
  glMatrixMode(GL_MODELVIEW);
}

void light_init()
{
    GLfloat mat_specular[]={1.0, 1.0, 1.0, 1.0};
    GLfloat mat_diffuse[]={0.0, 0.0, 1.0, 1.0};
    GLfloat mat_ambient[]={0.0, 0.5, 1.0, 1.0};
    GLfloat mat_shininess={100.0};
    GLfloat light_ambient[]={0.5, 0.5, 0.5, 1.0};
    GLfloat light_diffuse[]={1.0, 1.0, 1.0, 1.0};
    GLfloat light_specular[]={1.0, 1.0, 1.0, 1.0};

    GLfloat light_position[] = {0.0, 5.0, -2.0, 1.0};

/* set up ambient, diffuse, and specular components for light 0 */

    glLightfv(GL_LIGHT0, GL_AMBIENT, light_ambient);
    glLightfv(GL_LIGHT0, GL_DIFFUSE, light_diffuse);
    glLightfv(GL_LIGHT0, GL_SPECULAR, light_specular);
    glLightfv(GL_LIGHT0, GL_POSITION, light_position);

/* define material proerties for front face of all polygons */

    //glMaterialfv(GL_FRONT, GL_SPECULAR, mat_specular);
    //glMaterialfv(GL_FRONT, GL_AMBIENT, mat_ambient);
    //glMaterialfv(GL_FRONT, GL_DIFFUSE, mat_diffuse);
    //glMaterialf(GL_FRONT, GL_SHININESS, mat_shininess);

    glShadeModel(GL_SMOOTH); /*enable smooth shading */
    glEnable(GL_LIGHTING); /* enable lighting */
    glEnable(GL_LIGHT0);  /* enable light 0 */
    glEnable(GL_DEPTH_TEST); /* enable z buffer */
    glEnable(GL_COLOR_MATERIAL);
    glColorMaterial(GL_FRONT,GL_AMBIENT_AND_DIFFUSE);

    glClearColor (1.0, 1.0, 1.0, 1.0);
    glColor3f (1, 0.0, 0.0);
}

// A simple test for the mdr.driver animation
void test_driver()
{
  mdr.driver->snapshot();
  mdr.driver->test_angles();
  mdr.driver->snapshot(1);
  mdr.driver->reset_angles();
  mdr.driver->set_frames(1000);
}

void timer(int value);

// Initialize all of the objects to be drawn.
void scene_init()
{
  mdr.init();
  default_view();
}

// Initialize the objects to be drawn on the small window
void draw_small()
{
  mdr.redraw_small();
}

void timer(int value)
{
  cout << value << endl;
}
