/* plan.c

Laura Toma

What it does:  


*/

#include "geom.h"
#include "rtimer.h"

#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include <assert.h>

#ifdef __APPLE__
#include <GLUT/glut.h>
#else
#include <GL/glut.h>
#endif


GLfloat red[3] = {1.0, 0.0, 0.0};
GLfloat green[3] = {0.0, 1.0, 0.0};
GLfloat blue[3] = {0.0, 0.0, 1.0};
GLfloat black[3] = {0.0, 0.0, 0.0};
GLfloat white[3] = {1.0, 1.0, 1.0};
GLfloat gray[3] = {0.5, 0.5, 0.5};
GLfloat yellow[3] = {1.0, 1.0, 0.0};
GLfloat magenta[3] = {1.0, 0.0, 1.0};
GLfloat cyan[3] = {0.0, 1.0, 1.0};

GLint fillmode = 0;



/* forward declarations of functions */
void display(void);
void keypress(unsigned char key, int x, int y);
void main_menu(int value);
void timerfunc(); 



/* ******************** */
/* global variables */
/* ******************** */

const int WINDOWSIZE = 500; 

point2D start_pos, end_pos;  /* start and end position of the robot */

/* the current position of the robot.  Will be be updated by next_move() */
point2D current_pos; 

/* the array of obstacles */
int nobstacles;
polygon2D* obst;





void print_obstacle(polygon2D p) {
  printf("n=%d : ", p.n);
  int i;
  for (i=0; i<p.n; i++) {
    printf("(%d, %d) ", p.points[i].x, p.points[i].y);
  }
}

/* ****************************** */
/* create and initialize  the obstacles array from input file */
//note: the obstacles are in [0,WINDOWSIZE]x[0,WINDOWSIZE]
void initialize_scene(char* inputfname) {

  //here should read scene from file 


  //let's put something there 
  start_pos.x = 50; start_pos.y = 50; 
  end_pos.x=400; end_pos.y = 400; 
  current_pos = start_pos;

  nobstacles=1; 
  obst = (polygon2D*)malloc(nobstacles*sizeof(polygon2D));
  assert(obst); 

  //put something quick and easy in the first obstacle
  //the obstacles are in [0,WINDOWSIZE]x[0,WINDOWSIZE]
  obst[0].n = 4; 
  obst[0].points = (point2D*)malloc(obst[0].n*sizeof(point2D));
  assert(obst[0].points); 
  obst[0].points[0].x = 100;  obst[0].points[0].y=300; 
  obst[0].points[1].x = 300; obst[0].points[1].y=300; 
  obst[0].points[2].x = 300; obst[0].points[2].y=400; 
  obst[0].points[3].x = 100;  obst[0].points[3].y=400;

  
  printf("initialized %d obstacles:\n", nobstacles);
  int i;
  for (i=0; i<nobstacles; i++) {
    print_obstacle(obst[i]);
  }
}




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


  if (argc!=2) {
    printf("usage: %s fname..\n", argv[0]);
    exit(1);
  }
  initialize_scene(argv[1]);
  

  /* initialize GLUT  */
  glutInit(&argc, argv);
  glutInitDisplayMode(GLUT_SINGLE | GLUT_RGB);
  glutInitWindowSize(WINDOWSIZE, WINDOWSIZE);
  glutInitWindowPosition(100,100);
  glutCreateWindow(argv[0]);

  /* register callback functions */
  glutDisplayFunc(display); 
  glutKeyboardFunc(keypress);
  glutIdleFunc(timerfunc); 

  /* init GL */
  /* set background color black*/
  glClearColor(0, 0, 0, 0);   
  /* here we can enable depth testing and double buffering and so
     on */

  
  /* give control to event handler */
  glutMainLoop();
  return 0;
}


/* ****************************** */
/* this should triangulated the polygon and draw each triangle filled */
void draw_obstacle(polygon2D p){

  const int R= 1;
  //glPolygonMode(GL_FRONT_AND_BACK, GL_LINE);
  glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);

  //set color 
  glColor3fv(yellow);   
  
  glBegin(GL_POLYGON);
  int i; 
  for (i=0; i<p.n; i++) {
    //draw a small square centered at (points[i].x, points[i].y)
    glVertex2f(p.points[i].x, p.points[i].y);
  }
  glEnd();
}






/* ****************************** */
void display(void) {

  glClear(GL_COLOR_BUFFER_BIT);
  glMatrixMode(GL_MODELVIEW); 
  glLoadIdentity(); //clear the matrix


  /* the default GL window is [-1,1]x[-1,1] with the origin in the
     center the points are in the range (0,0) to (WINSIZE,WINSIZE), so
     they need to be mapped to [-1,1]x [-1,1] */
  glScalef(2.0/WINDOWSIZE, 2.0/WINDOWSIZE, 1.0);  
  glTranslatef(-WINDOWSIZE/2, -WINDOWSIZE/2, 0); 

  //draw start point 
  glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);
  glColor3fv(red);   
  glBegin(GL_POLYGON);
  int R=3; 
  glBegin(GL_POLYGON); 
  glVertex2f(start_pos.x -R, start_pos.y -R);
  glVertex2f(start_pos.x +R, start_pos.y -R);
  glVertex2f(start_pos.x +R, start_pos.y +R);
  glVertex2f(start_pos.x -R, start_pos.y +R);
  glEnd();

  //draw end point 
  glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);
  glColor3fv(green);   
  glBegin(GL_POLYGON); 
  glVertex2f(end_pos.x -R, end_pos.y -R);
  glVertex2f(end_pos.x +R, end_pos.y -R);
  glVertex2f(end_pos.x +R, end_pos.y +R);
  glVertex2f(end_pos.x -R, end_pos.y +R);
  glEnd();

  //draw current point

 glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);
 glColor3fv(blue);   
  glBegin(GL_POLYGON); 
  glVertex2f(current_pos.x -R, current_pos.y -R);
  glVertex2f(current_pos.x +R, current_pos.y -R);
  glVertex2f(current_pos.x +R, current_pos.y +R);
  glVertex2f(current_pos.x -R, current_pos.y +R);
  glEnd();
  
  //draw obstacles 
  int i;
  for (i=0; i<nobstacles; i++) {
    draw_obstacle(obst[i]);
  }

  /* execute the drawing commands */
  glFlush();
}



/* ****************************** */
void keypress(unsigned char key, int x, int y) {
  switch(key) {
  case 'q':
    exit(0);
    break;
  } 
}


/* Handler for window re-size event. Called back when the window first appears and
   whenever the window is re-sized with its new width and height */
void reshape(GLsizei width, GLsizei height) {  // GLsizei for non-negative integer
     
   // Set the viewport to cover the new window
   glViewport(0, 0, width, height);
 
   glMatrixMode(GL_PROJECTION);  // To operate on the Projection matrix
   glLoadIdentity();             // Reset
   gluOrtho2D(0.0, (GLdouble) width, 0.0, (GLdouble) height); 
}



/* this should do an a* search or something like that. right now it's
just moving up diagonally and not checking any obstacles
*/
point2D next_move() {

  point2D  next; 
  next.x = current_pos.x+1; 
  next.y = current_pos.y+1; 
  return next;
}


void timerfunc() {
  
  //static int lastFrameTime=0;  
  //note: a static variable, remembered from one call to the next
  //int now, elapsed_ms; 
  
  //now = glutGet (GLUT_ELAPSED_TIME); 
  //elapsed_ms = now - lastFrameTime; 
  //lastFrameTime=now; 
 
  
  current_pos = next_move(); 

  glutPostRedisplay(); 

}