/************************************************************************/
/************************************************************************/
// path1.c: generate a path through the marble maze.

/************************************************************************/
/************************************************************************/

#include "stdafx.h" /* Windows stuff, make empty stdafx.h under Linux */

/************************************************************************/

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

/************************************************************************/
/* DEFINES */

#define VERY_BIG_VALUE 1e30

/************************************************************************/
/* General Globals */

/************************************************************************/
/* GLUT/OPENGL Globals */

float view[4] = { -1.0, -1.0, 1.0, 1.0 };
GLsizei window_widthi, window_heighti;
GLfloat window_widthf, window_heightf;
float mouse_display[2] = { 0.0, 0.0 };

#define MAX_WINDOW_SIZE 400

/************************************************************************/
/* Globals */

BOARD a_board;
BOARD *the_board;

/************************************************************************/
/* The planning data structure */

#define RESOLUTION 100
/*
#define RESOLUTION 50
#define RESOLUTION 500
*/

typedef struct map
{
  float rect[4];
  float resolution[2];
  char obstacles[RESOLUTION][RESOLUTION];
  char holes[RESOLUTION][RESOLUTION];
  char goal[RESOLUTION][RESOLUTION];
  float one_step_cost[RESOLUTION][RESOLUTION];
  float temp[RESOLUTION][RESOLUTION];
  float value_function[RESOLUTION][RESOLUTION];
  float display[RESOLUTION][RESOLUTION][3];
}
MAP;

MAP a_map;
MAP *the_map = NULL;

/************************************************************************/
/************************************************************************/

void flush_graphics()
{
  glutPostRedisplay();
  glutMainLoopEvent();
}

/************************************************************************/

void display()
{
  int i, j;
  MAP *m;
  float x, y;

  m = the_map;

  glClear( GL_COLOR_BUFFER_BIT );

  for( i = 0; i < m->resolution[XX]; i++ )
    {
      x = (float) i;
      for( j = 0; j < m->resolution[YY]; j++ )
	{
	  y = (float) j;
	  glColor3fv( &(m->display[i][j][0]) );
	  /*
	  glColor3f( m->display[i][j][0],
		     m->display[i][j][1],
		     m->display[i][j][2] );
	  */
	  glRectf( x, y, x+1, y+1 );
	  /*
	  printf( "%d %d: %g %g %g %g %g\n", i, j, x, y,
		  m->display[i][j][0],
		  m->display[i][j][1],
		  m->display[i][j][2] );
	  */
	}
    }

  glutSwapBuffers();
}

/************************************************************************/

void init_display( MAP *m )
{
  int i, j;
  float increment, angle;

  view[LEFT] = 0;
  view[BOTTOM] = 0;
  view[RIGHT] = RESOLUTION;
  view[TOP] = RESOLUTION;

  glMatrixMode( GL_PROJECTION );
  glLoadIdentity();
  gluOrtho2D( view[LEFT], view[RIGHT], view[BOTTOM], view[TOP] );
}

/************************************************************************/
/* This needs to be fixed. */

void reshape( GLsizei w, GLsizei h )
{
  window_widthi = w;
  window_heighti = h;
  window_widthf = (GLfloat) w;
  window_heightf = (GLfloat) h;

  glMatrixMode( GL_PROJECTION );
  glLoadIdentity();
  /*
  if ( w <= h )
    gluOrtho2D( view[LEFT], view[RIGHT], 
		view[BOTTOM]*window_heightf/window_widthf,
		view[TOP]*window_heightf/window_widthf );
  else
    gluOrtho2D( view[LEFT]*window_widthf/window_heightf, 
		view[RIGHT]*window_widthf/window_heightf, 
		view[BOTTOM], view[TOP] );
  */
  gluOrtho2D( view[LEFT], view[RIGHT],
	      view[BOTTOM], view[TOP] );
  /* glMatrixMode( GL_MODELVIEW ); */
  glViewport( 0, 0, w, h );
}

/************************************************************************/

void idle()
{
  glutPostRedisplay();
}

/************************************************************************/
/************************************************************************/

void keypress( unsigned char key, int x, int y )
{
  if ( key == 'q' || key == 'Q' || key == '\27' )
    exit( 0 );
  mouse_display[XX] = x;
  mouse_display[YY] = y;
}

/************************************************************************/

void mouse( int button, int state, int x, int y )
{
  char b, d;
  if ( button == GLUT_LEFT_BUTTON )
    b = 'L';
  else if ( button == GLUT_RIGHT_BUTTON )
    b = 'R';
  else
    b = 'M';
  if ( state == GLUT_UP )
    d = 'U';
  else
    d = 'D';
  mouse_display[XX] = x;
  mouse_display[YY] = y;
}

/************************************************************************/

void mouse_motion( int x, int y )
{
  mouse_display[XX] = x;
  mouse_display[YY] = y;
}

/************************************************************************/

int init_glut( int argc, char **argv, float ratio )
{
  int wx, wy;

  if ( ratio < 1.0 )
    {
      wx = MAX_WINDOW_SIZE;
      wy = wx*ratio;
    }
  else
    {
      wy = MAX_WINDOW_SIZE;
      wx = wy/ratio;
    }

  glutInit( &argc, argv );
  glutInitDisplayMode( GLUT_RGB | GLUT_DOUBLE );
  glutInitWindowPosition( 0, 0 );
  glutInitWindowSize( wx, wy );
  glutCreateWindow( "path1" );
  glutDisplayFunc( display );
  glutReshapeFunc( reshape );
  glutIdleFunc( idle );
  glutKeyboardFunc( keypress );
  glutMouseFunc( mouse );
  /*
  glutMotionFunc( mouse_motion );
  glutPassiveMotionFunc( mouse_motion );
  */
  return TRUE;
}

/************************************************************************/
/************************************************************************/

void init_board_map( BOARD *b, MAP *m )
{
  int i, j;

  for ( i = 0; i < 4; i++ )
    m->rect[i] = b->board.p[i];
  m->resolution[XX] = RESOLUTION;
  m->resolution[YY] = RESOLUTION;
  for( i = 0; i < m->resolution[XX]; i++ )
    {
      for( j = 0; j < m->resolution[YY]; j++ )
	{
	  m->holes[i][j] = 0;
	  m->obstacles[i][j] = 0;
	  m->goal[i][j] = 0;
	  m->one_step_cost[i][j] = 0;
	  m->value_function[i][j] = VERY_BIG_VALUE;
	  m->display[i][j][0] = 0.0;
	  m->display[i][j][1] = 0.0;
	  m->display[i][j][2] = 0.0;
	}
    }
  flush_graphics();
}

/************************************************************************/

int index_to_board_position( MAP *m, int ix, int iy, 
			     float *x, float *y )
{
  *x = m->rect[0] + ix*(m->rect[2] - m->rect[0])/RESOLUTION;
  *y = m->rect[1] + iy*(m->rect[3] - m->rect[1])/RESOLUTION;
}

/************************************************************************/

int board_position_to_index( MAP *m, float x, float y, 
			     int *ix, int *iy )
{
  *ix = RESOLUTION*(x - m->rect[0])/(m->rect[2] - m->rect[0]);
  *iy = RESOLUTION*(y - m->rect[1])/(m->rect[3] - m->rect[1]);
}

/************************************************************************/

void path_map_obstacles( BOARD *b, MAP *m )
{
  int i, j;
  int ix, iy;
  float position[2];

  for( ix = 0; ix < m->resolution[XX]; ix++ )
    {
      for( iy = 0; iy < m->resolution[YY]; iy++ )
	{
	  index_to_board_position( m, ix, iy,
				   &(position[XX]), &(position[YY]) );
	  if ( in_obstacle( b, position ) )
	    {
	      m->obstacles[ix][iy] = 1;
	      m->value_function[ix][iy] = VERY_BIG_VALUE;
	      m->display[ix][iy][0] = 1.0;
	      m->display[ix][iy][1] = 1.0;
	      m->display[ix][iy][2] = 1.0;
	    }
	  else
	    {
	      m->obstacles[ix][iy] = 0;
	      m->display[ix][iy][0] = 0.0;
	      m->display[ix][iy][1] = 0.0;
	      m->display[ix][iy][2] = 0.0;
	    }
	}
    }
  flush_graphics();
}

/************************************************************************/

void path_map_holes( BOARD *b, MAP *m )
{
  int i, j;
  int ix, iy;
  float position[2];

  for( ix = 0; ix < m->resolution[XX]; ix++ )
    {
      for( iy = 0; iy < m->resolution[YY]; iy++ )
	{
	  if ( m->obstacles[ix][iy] > 0 )
	    {
	      continue;
	    }
	  index_to_board_position( m, ix, iy,
				   &(position[XX]), &(position[YY]) );
	  if ( in_hole( b, position ) )
	    {
	      m->holes[ix][iy] = 1;
	      m->value_function[ix][iy] = VERY_BIG_VALUE;
	      m->display[ix][iy][0] = 0.0;
	      m->display[ix][iy][1] = 0.0;
	      m->display[ix][iy][2] = 1.0;
	    }
	  else
	    {
	      m->holes[ix][iy] = 0;
	    }
	}
    }
  flush_graphics();
}

/************************************************************************/

void initialize_value_function( BOARD *b, MAP *m )
{
  int i, j;
  int ix, iy;
  float position[2];

  for( ix = 0; ix < m->resolution[XX]; ix++ )
    {
      for( iy = 0; iy < m->resolution[YY]; iy++ )
	{
	  if ( m->obstacles[ix][iy] > 0 )
	    continue;
	  if ( m->holes[ix][iy] > 0 )
	    continue;
	  index_to_board_position( m, ix, iy,
				   &(position[XX]), &(position[YY]) );
	  if ( in_goal( b, position ) )
	    {
	      m->goal[ix][iy] = 1;
	      m->value_function[ix][iy] = 0.0;
	      m->display[ix][iy][0] = 1.0;
	    }
	  else
	    {
	    }
	}
    }
  flush_graphics();
}

/************************************************************************/

int grow_obstacle( MAP *m, int jx, int jy )
{
  int ix, iy;
  float biggest_value = -VERY_BIG_VALUE;
  float decay;

  for( ix = jx - 1; ix <= jx + 1; ix++ )
    {
      if ( ix < 0 )
	continue;
      if ( ix >= RESOLUTION )
	continue;
      for( iy = jy - 1; iy <= jy + 1; iy++ )
	{
	  if ( iy < 0 )
	    continue;
	  if ( iy >= RESOLUTION )
	    continue;
	  if ( m->obstacles[ix][iy] > 0 )
	    continue;
	  if ( m->goal[ix][iy] > 0 )
	    continue;
	  if ( biggest_value < m->temp[ix][iy] )
	    {
	      /*
	      printf( "growing: %d %d: %d %d: %g %g\n", jx, jy, ix, iy, 
		      biggest_value, m->temp[ix][iy] );
	      */
	      biggest_value = m->temp[ix][iy];
	    }
	}
    }
  /* Trying to get same decay rate per meter for different resolutions.
     Need to take into account board size, etc. */
  decay = 1.0 - 0.1*(100.0/RESOLUTION);
  m->one_step_cost[jx][jy] = decay*biggest_value;
  m->display[jx][jy][0] = m->one_step_cost[jx][jy];
  m->display[jx][jy][1] = 0;
  m->display[jx][jy][2] = m->one_step_cost[jx][jy];
}

/************************************************************************/

void calculate_one_step_costs( MAP *m )
{
  /* Here is where we expand holes */
  int i, j;
  int ix, iy;
  int n_growth_cycles;

  /* First copy the holes */
  for( ix = 0; ix < m->resolution[XX]; ix++ )
    {
      for( iy = 0; iy < m->resolution[YY]; iy++ )
	{
	  if ( m->obstacles[ix][iy] > 0 )
	    continue;
	  if ( m->holes[ix][iy] > 0 )
	    m->one_step_cost[ix][iy] = 1.0;
	}
    }

  /* Trying to grow obstacles same distance independent of resolution.
   Need to take into account size of board. */
  n_growth_cycles = 5*(RESOLUTION/100.0);
  if ( n_growth_cycles < 1 )
    n_growth_cycles = 1;

  for( i = 0; i < n_growth_cycles; i++ )
    {
      /* Copy to temp. */
      for( ix = 0; ix < m->resolution[XX]; ix++ )
	{
	  for( iy = 0; iy < m->resolution[YY]; iy++ )
	    {
	      m->temp[ix][iy] = m->one_step_cost[ix][iy];
	    }
	}

      /* Grow obstacles */
      printf( "Growing obstacles.\n" );
      for( ix = 0; ix < m->resolution[XX]; ix++ )
	{
	  for( iy = 0; iy < m->resolution[YY]; iy++ )
	    {
	      if ( m->obstacles[ix][iy] > 0 )
		continue;
	      if ( m->holes[ix][iy] > 0 )
		continue;
	      if ( m->goal[ix][iy] > 0 )
		continue;
	      grow_obstacle( m, ix, iy );
	    }
	}
      flush_graphics();
    }
}

/************************************************************************/

int update_cell( MAP *m, int jx, int jy )
{
  int ix, iy;
  float best_value = VERY_BIG_VALUE;
  float old_value;

  for( ix = jx - 1; ix <= jx + 1; ix++ )
    {
      if ( ix < 0 )
	continue;
      if ( ix >= RESOLUTION )
	continue;
      for( iy = jy - 1; iy <= jy + 1; iy++ )
	{
	  if ( iy < 0 )
	    continue;
	  if ( iy >= RESOLUTION )
	    continue;
	  if ( m->obstacles[ix][iy] > 0 )
	    continue;
	  if ( m->holes[ix][iy] > 0 )
	    continue;
	  if ( best_value > m->value_function[ix][iy] )
	    {
	      /*
	      printf( "updating: %d %d: %d %d: %g %g\n", jx, jy, ix, iy, 
		      best_value, m->value_function[ix][iy] );
	      */
	      best_value = m->value_function[ix][iy];
	    }
	}
    }
  best_value += m->one_step_cost[jx][jy] + 1.0/RESOLUTION;
  old_value = m->value_function[jx][jy];
  m->value_function[jx][jy] = best_value;
  if ( best_value < old_value )
    return 1;
  else
    return 0;
}

/************************************************************************/

/* Good for resolution 100 */
float value_scale_factor = 20.0;

int do_sweep( MAP *m )
{
  int ix, iy;
  int n_changes = 0;
  float value;

  /* Reversing the order of these indices on every sweep will speed
     up the value function calculation */
  for( ix = 0; ix < m->resolution[XX]; ix++ )
    {
      for( iy = 0; iy < m->resolution[YY]; iy++ )
	{
	  if ( m->obstacles[ix][iy] > 0 
	       || m->holes[ix][iy] > 0 )
	    continue;
	  if ( m->goal[ix][iy] > 0 )
	    {
	      m->display[ix][iy][0] = 1.0;
	      m->display[ix][iy][1] = 0.0;
	      m->display[ix][iy][2] = 0.0;
	      continue;
	    }
	  n_changes += update_cell( m, ix, iy );
	  value = m->value_function[ix][iy];
	  /*
	  if ( value > value_scale_factor )
	    value_scale_factor = value;
	  */
	  value /= value_scale_factor;
	  if ( value > 1.0 )
	    value = 1.0;
	  m->display[ix][iy][0] = 0.0;
	  m->display[ix][iy][1] = value;
	  m->display[ix][iy][2] = 0.0;
	}
    }
  flush_graphics();
  printf( "sweep: %d changes.\n", n_changes );
  return n_changes;
}

/************************************************************************/

int plan_path( MAP *m, int jx, int jy, char *filename )
{
  int ix, iy;
  float best_value;
  float best_x, best_y;
  FILE *stream;
  float x, y;

  stream = fopen( filename, "w" );
  if ( stream == NULL )
    {
      fprintf( stderr, "Can't open file %s for write.\n", filename );
      exit( -1 );
    }

  for( ; ; )
    {
      index_to_board_position( m, jx, jy, &x, &y );
      fprintf( stream, "%g %g\n", x, y );
      if ( m->goal[jx][jy] )
	{
	  printf( "%d %d at goal\n", jx, jy );
	  break;
	}
      /*
      printf( "Planning path at %d %d\n", jx, jy );
      */
      best_value = VERY_BIG_VALUE;
      for( ix = jx - 1; ix <= jx + 1; ix++ )
	{
	  if ( ix < 0 )
	    continue;
	  if ( ix >= RESOLUTION )
	    continue;
	  for( iy = jy - 1; iy <= jy + 1; iy++ )
	    {
	      if ( iy < 0 )
		continue;
	      if ( iy >= RESOLUTION )
		continue;
	      if ( m->obstacles[ix][iy] > 0 )
		continue;
	      if ( m->holes[ix][iy] > 0 )
		continue;
	      if ( best_value > m->value_function[ix][iy] )
		{
		  best_value = m->value_function[ix][iy];
		  best_x = ix;
		  best_y = iy;
		}
	    }
	}
      m->display[jx][jy][0] = 1.0;
      m->display[jx][jy][1] = 0.0;
      m->display[jx][jy][2] = 0.0;
      jx = best_x;
      jy = best_y;
    }

  fclose( stream );
}

/************************************************************************/

int main( int argc, char **argv )
{
  int wx, wy;
  float ratio;
  BOARD *b;
  MAP *m;
  int n_changes;
  int ix, iy;

  /* Set up a board */
  b = the_board = &a_board;
  initialize_maze( "board2", b );

  /* Set up planning map */
  m = the_map = &a_map;

  init_glut( argc, argv,
	     ( b->board.p[TOP] - b->board.p[BOTTOM] )
	     /( b->board.p[RIGHT] - b->board.p[LEFT] ) );
  init_display( m );

  init_board_map( b, m );
  path_map_obstacles( b, m );
  path_map_holes( b, m );
  initialize_value_function( b, m );
  calculate_one_step_costs( m );
  printf( "Press return to continue.\n" );
  getchar();
  for( ; ; )
    {
      n_changes = do_sweep( m );
      if ( n_changes == 0 )
	break;
    }
  board_position_to_index( m, b->start[XX], b->start[YY], &ix, &iy );
  plan_path( m, ix, iy, "path1.new" );

  printf( "DONE\n" );
  glutMainLoop();
  return 0;
}

/************************************************************************/