/*
 *          File: nomadTrack.cc
 *       Authors: Paul Ruvolo, Conor Sen, Ryan Gibson
 * Last Modified: 9 May, 2003
 */

#include "GL/glut.h"
#include <stream.h>
#include <fstream.h>
#include <stdio.h>
#include <math.h>

#define WIDTH 160
#define HEIGHT 120

float MIN(float a, float b, float c);
float MAX(float a, float b, float c);
void RGBtoHSV( float r, float g, float b, float *h, float *s, float *v );
float scorePixel(int r,
		 int g,
		 int b,
		 int i,
		 int j,
		 int lastHeight,
		 int lastWidth);
/* 
 * This is a global variable used to hold images
 * The "3" refers to the three color bands available.
 *
 *   Each pixel has a red (0)
 *                  green (1)
 *              and  blue (2) value.
 *
 *   Each of these values is between 0 and 255, inclusive.
 *
 * GLubyte is OpenGL's unsigned int.
 */

GLubyte im[HEIGHT][WIDTH][3];
char imageFileName[100];           // a string to hold the image file name
int currentImageNumber = 0;        // the image numbers go up by 6 each frame
int continuousPlay = 0;            // for continuous sequence playing



/*
 * These are forward declaration of functions used later
 */

void getImage(int imageNumber);    // loads the next image into gloabl "im"

void init();                       // OpenGL initialization
void display();                    // OpenGL general display routine
void reshape(int w, int h);        // OpenGL window-resize routine
void mouse(int button, int state, int x, int y);   // mouse-event handling
void keyboard(unsigned char key, int x, int y);    // keyboard-event handling



/*
 * This is the main routine; it sets up the OpenGL
 * settings for displaying images. In reality this
 * is an event-driven program, so that the control
 * comes from the routines
 *
 * keyboard(key,x,y)
 *
 * and
 *
 * mouse(button,state,x,y)
 */


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

  /* 
   * If you're curious as to what is going on in OpenGL,
   * one good source of information is http://www.opengl.org
   */
	
	/*  set up window */

	glutInit(&argc, argv);
	glutInitDisplayMode(GLUT_RGB | GLUT_DOUBLE );
	glutInitWindowSize(160,120);
	glutInitWindowPosition(100,100);
	glutCreateWindow("Object Tracking");

	/* initialize state */

	init();

	/* register enevt-handling functions */

	glutDisplayFunc(display);
	glutReshapeFunc(reshape);
	glutMouseFunc(mouse);
	glutKeyboardFunc(keyboard);

	/* wait until something happens */

	glutMainLoop();

	return 1;
}




/*
 * This routine provides additional initialization for
 * various OpenGL parameters.
 */

void 
init()
{
	/*  sets clear color to black  */

	glClearColor(0.0,0.0,0.0,0.0);

	/*  define the viewing system */ 
	glMatrixMode(GL_PROJECTION);
	glLoadIdentity();
	gluOrtho2D(0.0, (GLfloat)WIDTH, 0.0, (GLfloat)HEIGHT);
	glMatrixMode(GL_MODELVIEW);
	glLoadIdentity();
	glShadeModel(GL_FLAT);
	
        glPixelStorei(GL_UNPACK_ALIGNMENT,1);
	
}



/*
 * A routine to get the next image from the files
 * in /cs/cs154/as/problemset4/redSequence
 */

void
getNextImage()
{
  /*
   * set up the next file to read
   */

  sprintf(imageFileName,
          "/mnt/cs/cs154/as/problemset4/redSequence/frame.%04d.ppm",
          currentImageNumber);
  cout << "Reading image " << imageFileName << endl;
  ifstream inf(imageFileName);

  /* 
   * There is a 15-byte header
   */


  unsigned char c;
  for (int k=0 ; k<15 ; ++k) {
    inf.get(c);
    // cout << "k is " << k << " and c is " << (int)c << "   " << c << endl;
  }

  /*
   * and then we get to the image...
   */

  for (int j=HEIGHT - 1 ; j>=0 ; --j)
  for (int i=0 ; i<WIDTH ; ++i)
  {
    inf.get(im[j][i][0]);
    inf.get(im[j][i][1]);
    inf.get(im[j][i][2]);
  }

  /*
   * we increase the image number so that next time,
   * the next image will be grabbed
   */

  currentImageNumber += 6;  // each image name increases by 6

}





/*
 * drawPlus() is a function for putting a 9 pixel
 * + sign at a particular point in the image im
 *
 * The first four lines crop the + sign appropriately.
 */

void
drawPlus(int y, int x)
{
  int ymin = y-4 > 0 ? y-4 : 0;
  int ymax = y+4 < HEIGHT ? y+4 : HEIGHT-1;
  int xmin = x-4 > 0 ? x-4 : 0;
  int xmax = x+4 < WIDTH ? x+4 : WIDTH-1;

  for (int j=ymin ; j<=ymax ; ++j) 
    im[j][x][0] = im[j][x][1] = im[j][x][2] = 255;  // set r,g,b to max

  for (int j=xmin ; j<=xmax ; ++j) 
    im[y][j][0] = im[y][j][1] = im[y][j][2] = 255;  // set r,g,b to max
}





/*
 * Here is processTheImage() -- this is the function
 * you should use in order to track the red frisbee from
 * frame to frame. (Certainly you should feel free to 
 * add other functions that will be called from this one.)
 *
 * You may keep any state about the frisbee around (OpenGL
 * tends to use global variables, though you can use static
 * variables in this function if you're very opposed to
 * globals.
 *
 * For example, you may want to keep the position of the frisbee
 * in the previous frame as a starting point for locating it in the
 * next one.
 *
 * In order to indicate where your algorithm believes the
 * frisbee is, you should draw a white + sign at the
 * location estimate you compute. As a start, the routine
 * currently draws a white + sign in the middle of the window.
 *
 * Feel free to draw (or print out to the console) whatever other 
 * graphics (or textual information) you think might be helpful.
 *
 * Keep in mind that you can program keyboard input and mouse
 * input to help you automate tasks (in the "keyboard" and "mouse"
 * functions below.
 *
 */

float scorePixel(int r,
		 int g,
		 int b,
		 int i,
		 int j,
		 int lastHeight,
		 int lastWidth) {
  float h, s, v;
  float HUE_THRESHOLD = 3.0;
  float SAT_THRESHOLD = 0.5;
  float MOVEMENT_MODEL = 15.0;

  RGBtoHSV(r, g, b, &h, &s, &v);
  if ((h < HUE_THRESHOLD || h > 360 - HUE_THRESHOLD) &&
      s > SAT_THRESHOLD) {
    if (lastHeight != -1) {
      float dist = sqrt((i - lastHeight)*(i - lastHeight) + (j - lastWidth)*(j - lastWidth));
      if (dist == 0) {
	return 2.0;
      } else if (dist <= MOVEMENT_MODEL) {
	return 1/(sqrt(dist));
      } else {
	return 1/(4*sqrt(dist));
      }
    } else {
      return 1.0;
    }
  } else {
    return 0.0;
  }
}

void
processTheImage()
{
  static int lastHeight = -1;
  static int lastWidth = -1;
  int i, j, k, l;
  float h, s, v;
  float AVG_HEIGHT = 0;
  float AVG_WIDTH = 0;
  float numPixels = 0;
  int cellSize = 30;

  int DISTANCE_THRESHOLD = 40;
  float maxScore = -1.0;
  int maxHeight = -1;
  int maxWidth = -1;

  float** pixelScores = new float*[HEIGHT];
  for (i = 0; i < HEIGHT; i++) {
    pixelScores[i] = new float[WIDTH];
    for (j = 0; j < WIDTH; j++) {
      pixelScores[i][j] = scorePixel(im[i][j][0], 
				   im[i][j][1],
				   im[i][j][2],
				   i,
				   j,
				   lastHeight,
				   lastWidth);
    }
  }

  if (lastHeight == -1) {
    for (i = 0; i < HEIGHT - cellSize; i++) {
      for (j = 0; j < WIDTH - cellSize; j++) {
	float totalScore = 0.0;

	for (k = 0; k < cellSize; k++) {
	  for (l = 0; l < cellSize; l++) {
	    totalScore += pixelScores[i+k][j+l];
	  }
	}
	if (totalScore > maxScore) {
	  maxHeight = i;
	  maxWidth = j;
	  maxScore = totalScore;
	}
      }
    }
  } else {
    for (i = lastHeight - DISTANCE_THRESHOLD; i < lastHeight + DISTANCE_THRESHOLD; i++) {
      for (j = lastWidth - DISTANCE_THRESHOLD; j < lastWidth + DISTANCE_THRESHOLD; j++) {
	float totalScore = 0.0;

	if (i > 0 && i < HEIGHT - cellSize && j > 0 && j < WIDTH - cellSize) {
	  for (k = 0; k < cellSize; k++) {
	    for (l = 0; l < cellSize; l++) {
	      totalScore += pixelScores[i+k][j+l];
	    }
	  }
	  if (totalScore > maxScore) {
	    maxHeight = i;
	    maxWidth = j;
	    maxScore = totalScore;
	  }
	}
      }
    }
  }
  float currentHeight = 0.0;
  float currentWidth = 0.0;

  for (k = 0; k < cellSize; k++) {
    for (l = 0; l < cellSize; l++) {
      currentHeight += pixelScores[maxHeight+k][maxWidth+l]*(maxHeight+k);
      currentWidth += pixelScores[maxHeight+k][maxWidth+l]*(maxWidth+l);
    }
  }
  maxHeight = (int) (currentHeight / maxScore);
  maxWidth = (int) (currentWidth / maxScore);
  lastHeight = maxHeight;
  lastWidth = maxWidth;
  drawPlus(maxHeight, maxWidth);

  /* The image is the global variable "im" */

  /*
   * some code to seek out red pixels and find
   * an estimate of their center should go here
   */

  /* Right now, we estimate that the frisbee is in the
     center of the image */
  for (i = 0; i < HEIGHT; i++) {
    delete[] pixelScores[i];
  }
  delete[] pixelScores;

}




/*
 * The following function gets called in response to
 * a glutPostRedisplay() call.
 */

void 
display()
{
  glClear(GL_COLOR_BUFFER_BIT);
  getNextImage();                  // gets the next image from file

  /*
   * Here is the routine in which you should put all
   * of your processing. Because OpenGL tends to use
   * global variables to hold state (even though this
   * would not pass muster in CS70), I would encourage
   * it for this assignment.
   */

  processTheImage();              
  

  glRasterPos2i(0,0);
  glDrawPixels(WIDTH,HEIGHT,GL_RGB,GL_UNSIGNED_BYTE,im);   // draws it
  glutSwapBuffers();            // used to support double buffering
}






/*
 * an unused function for reshaping the window when
 * a user resizes it
 */

void 
reshape(int w, int h)
{
	/* width and height store current window dimensions */

	/*  map the projection plane to the entire window */

	// glViewport(0,0,WIDTH,HEIGHT);

  /*
   * We ignore reshape commands and assume the user will
   * not try to resize the window.
   */

}


void
idlePlay()
{
  glutPostRedisplay();
}


/*
 * function to handle keyboard inputs
 *
 * right now, q -- quits
 *            n -- moves to next frame
 *            p -- toggles continuous image updating
 */

void 
keyboard(unsigned char key, int x, int y)
{
  switch (key) 
  { 

   /*
    * I cannot find a glut function that
    * lets you more cleanly exit the loop. 
    * Several online glut references say simply
    * to call exit, so we will... .
    */

  case 'q':
  case 'Q':
    exit(0);
    break;

  case 'n':
  case 'N':
    glutPostRedisplay();
    break;

  case 'p':
  case 'P':
    continuousPlay = 1 - continuousPlay;
    if (continuousPlay)
      glutIdleFunc(idlePlay);
    else 
      glutIdleFunc(NULL);
    break;

  default:
    cout << "Key pressed was " << key;
    cout << " at (" << x << "," << y << ")\n";
    break;
  }

	return;
}





/*
 * function to handle mouse inputs
 * currently unused, but the mouse events are
 * printed to make it simple to incorporate, if
 * you'd like to
 */

void 
mouse(int button, int state, int x, int y)
{
  /*

  if (button == GLUT_LEFT_BUTTON) 
  	{
	  cout << "Left ";
 	}

  if (button == GLUT_MIDDLE_BUTTON)
	{
	  cout << "Right ";
 	}

  if (button == GLUT_RIGHT_BUTTON)
	{
	  cout << "Right ";
 	}

  if (state == GLUT_DOWN) 
 	{
	  cout << "button pressed ";
	}

  if (state == GLUT_UP) 
 	{
	  cout << "button released ";
	}

  cout << "at (" << x << "," << y << ")\n";
  
  */
 
  if (state == GLUT_DOWN) 
  {
    float h, s, v;

    RGBtoHSV(im[HEIGHT-y][x][0],
	     im[HEIGHT-y][x][1],
	     im[HEIGHT-y][x][2],
	     &h,
	     &s,
	     &v);
    cout << "Pixel at (" << x << "," << y << ")  is  ( r, g, b ) = (";
    cout << (int)im[HEIGHT-y][x][0] << ", ";
    cout << (int)im[HEIGHT-y][x][1] << ", ";
    cout << (int)im[HEIGHT-y][x][2] << " )\n";
    cout << "Pixel at (" << x << "," << y << ")  is  ( h, s, v ) = (";
    cout << h << ", ";
    cout << s << ", ";
    cout << v << " )\n";
  }

  return;
}


float MIN(float a, float b, float c) {
  if (a < b && a < c) {
    return a;
  }
  if (b < c) {
    return b;
  }
  return c;
}


float MAX(float a, float b, float c) {
  if (a > b && a > c) {
    return a;
  }
  if (b > c) {
    return b;
  }
  return c;
}

// stolen from http://www.cs.rit.edu/~ncs/color/t_convert.html
// r,g,b values are from 0 to 1
// h = [0,360], s = [0,1], v = [0,1]
//              if s == 0, then h = -1 (undefined)

void RGBtoHSV( float r, float g, float b, float *h, float *s, float *v )
{
        float min, max, delta;

	r /= 256.0;
	g /= 256.0;
	b /= 256.0;

        min = MIN( r, g, b );
        max = MAX( r, g, b );
        *v = max;                               // v

        delta = max - min;

        if( max != 0 )
                *s = delta / max;               // s
        else {
                // r = g = b = 0                // s = 0, v is undefined
                *s = 0;
                *h = -1;
                return;
        }

        if( r == max )
                *h = ( g - b ) / delta;         // between yellow & magenta
        else if( g == max )
                *h = 2 + ( b - r ) / delta;     // between cyan & yellow
        else
                *h = 4 + ( r - g ) / delta;     // between magenta & cyan

        *h *= 60;                               // degrees
        if( *h < 0 )
                *h += 360;

}