/**************************************************************************
              	         N E A R D E A R . C
***************************************************************************/
/*****************************************************************************
FILENAME   : neardear.c
PURPOSE    : Medial Axis Functions (minimally distant border points)
*****************************************************************************/

#include </sys/h/types.h>
#include </usr/include/curses.h>
#include <stdio.h>
#include <math.h>
#include "isthdefs.h"
#include "piece.h"
/*#include </usr/users/header/arrkey.h> */

#define EUCLID  1      /* use euclidean distances */

extern WINDOW  *funcwin;
extern int currpiece;
extern int inverse_isthmus;
extern int no_thinning;
extern int no_circle;
extern int radius;

extern char strg[79];
extern int image[512][512];
extern int temp_image[512][512];
extern int border_x[512][512];
extern int border_y[512][512];

int border_len = 1;
int robot_path_planning = NO;

int max_elevation = 0;
int nd_x[2500], nd_y[2500], nd_len;

/*****************************************************************************
                    NEAREST AND DEAREST BORDER POINTS
*****************************************************************************/
near_and_dear_pts(minx,miny,maxx,maxy)
int minx,miny,maxx,maxy;
{
   int c, r, j,i,e, z;
   int y, looky;
   int nadbpx, nadbpy;
   int deltax, deltay;
   
   sprintf (strg, "Euclidean Elevation Map -> ");  
   printit(strg); 

   max_elevation = 0;  
  
  deltax = (maxx - minx) + 1;
  deltay = (maxy - miny) + 1; 
  y = 0;
  for (j=miny; j<=maxy; j++)
  {
    VIS$READ_HORIZONTAL_LINE (minx, j, deltax+10, image[y]);
    y++;
  }
  if (!no_circle)
     euclid_lut (radius-1, 11); 
  
  
  for (j=1; j<=deltay; j++)
  {
   for (i=1; i<=deltax; i++)
   {
    if (image[j][i] != 0)
    {
     if ( image[j][i-1] == 0   || image[j-1][i] == 0 
       || image[j-1][i-1] == 0 || image[j-1][i+1] == 0
       || image[j][i+1] == 0   || image[j+1][i] == 0
       || image[j+1][i+1] == 0 || image[j+1][i-1] == 0)
     {  
	 border_x[j][i] =  i;
         border_y[j][i] =  j;
         temp_image[j][i]=63; 
     } 
     else /* image[j][i] is not a border points */
     {
       temp_image[j][i] = top_euclidean (j,i-1, j-1,i, j-1,i-1, j-1,i+1,
				  &nadbpx, &nadbpy); 
       /*temp_image[j][i] = (temp_image[j][i] % 253) + 1; */
       border_x[j][i] =  nadbpx;
       border_y[j][i] =  nadbpy;
     } /* end else */
    } /* endif image[j][i] != 0 */
    
    else /* else  image[j][i] == 0 */
    { 
      border_x[j][i] = 0; 
      border_y[j][i] = 0;
      temp_image[j][i]=254;
    }
   } /* endfor i */
   for (i=deltax-2; i>=1; i--)
   {
      do_near_2_mins(i,j, i+1,j); 
   }    

   VIS$WRITE_HORIZONTAL_LINE (minx, j + miny, deltax, temp_image[j]);
 } /* end for loop */

  if (inverse_isthmus)
     temp_image[j][0]=255; 
  else
     temp_image[j][0]=0; 
  for (j=deltay; j >= 1; j--)
  {
   for (i=1; i <= deltax; i++)
   {
    if (image[j][i] != 0)
    {
     if ( image[j][i-1] == 0   || image[j-1][i] == 0 
       || image[j-1][i-1] == 0 || image[j-1][i+1] == 0
       || image[j][i+1] == 0   || image[j+1][i] == 0
       || image[j+1][i+1] == 0 || image[j+1][i-1] == 0)
     { 
	 border_x[j][i] =  i;
         border_y[j][i] =  j;
	 temp_image[j][i]=63; 
#ifdef ROGER
         if (robot_path_planning)
	   load_inverse_border(minx+i, miny+j, border_len++, currpiece);
#endif
     }
     else
     { 
       temp_image[j][i] = bot_euclidean (j,i-1, j+1,i, j+1,i+1, j+1,i-1,
				  &nadbpx, &nadbpy); 
       temp_image[j][i] = round (sqrt( (float)temp_image[j][i]) ); /* RWW */
       if (temp_image[j][i] > max_elevation) 
          max_elevation = temp_image[j][i];
       border_x[j][i] =  nadbpx;
       border_y[j][i] =  nadbpy;
     }
    } /* endif border_x */
    else
    { 
      border_x[j][i] = 0; 
      border_y[j][i] = 0;
      temp_image[j][i]=0;
    }
   } /* endfor i */
   for (i=deltax-2; i>=1; i--)
   {
      do_near_2_mins(i,j, i+1,j); 
   }    
    
   VIS$WRITE_HORIZONTAL_LINE (minx, j + miny, deltax, temp_image[j]);
  } /* end loop */
  
#ifdef ROGER
  dump_image (deltax, deltay, minx, maxx, miny, maxy);
  waitforkey(); 
#endif
   
   sprintf (strg, "Max elevation is %d", max_elevation);
    
   /*print_dist_title();  */
   /* lut_roger(max_elevation);  /* fix the code to get max_elev*/
  
  sprintf (strg, "Finished EEM MAP"); 

  skel_luts(6);  /* 11 20 */
  sprintf (strg, "will call paint medial"); 
 
  skel_paint_medial (deltax, deltay, minx, miny, maxx, maxy);  
  
  sprintf (strg, "Finished skeleton"); 
#ifdef ROGER 
  printit(strg); 
  waitforkey(); 
#endif  
  
  if (robot_path_planning)
  {   show_inv (currpiece);
      waitforkey(); 
  } 
    
  if (!no_thinning)
  {
     do_thining (deltax, deltay, minx, miny, maxx, maxy);
     printf ("after thinning \n"); 
     skel_luts(6);  /* 11 20 */
  }
  else
  { 
    looky = ((maxy - miny) * PERCENT /* was0.33*/) + miny; /*was miny SAT*/ 
    VIS$BORDER_FOLLOW (&nd_len,nd_x,nd_y,254,255,minx,looky,maxx,maxy,5);
  }

} /* end function */


/*****************************************************************************
                       BOT_EUCLIDEAN                  
*****************************************************************************/
bot_euclidean (y1,x1, y2,x2, y3,x3, y4,x4, nadbpx, nadbpy)
int y1, x1, y2, x2, y3, x3, y4, x4, *nadbpx, *nadbpy;
{
  int x,y;
  int d1, d2, d3, d4;
  int p, p2, dd, ee, self, total;
  
  x = x2;
  y = y1;

#ifdef EUCLID 
  d1 = (border_x[y1][x1] - x) * (border_x[y1][x1] - x) + 
       (border_y[y1][x1] - y) * (border_y[y1][x1] - y); 
  d2 = (border_x[y2][x2] - x) * (border_x[y2][x2] - x) + 
       (border_y[y2][x2] - y) * (border_y[y2][x2] - y); 
  d3 = (border_x[y3][x3] - x) * (border_x[y3][x3] - x) + 
       (border_y[y3][x3] - y) * (border_y[y3][x3] - y); 
  d4 = (border_x[y4][x4] - x) * (border_x[y4][x4] - x) + 
       (border_y[y4][x4] - y) * (border_y[y4][x4] - y); 
#endif
#ifdef CITY_BLOCK 
    d1 = city_block_dist (border_x[y1][x1],border_y[y1][x1], x,y ); 
    d2 = city_block_dist (border_x[y2][x2],border_y[y2][x2], x,y ); 
    d3 = city_block_dist (border_x[y3][x3],border_y[y3][x3], x,y ); 
    d4 = city_block_dist (border_x[y4][x4],border_y[y4][x4], x,y ); 
#endif
#ifdef CHESSBOARD 
    d1 = chessboard_dist (border_x[y1][x1],border_y[y1][x1], x,y ); 
    d2 = chessboard_dist (border_x[y2][x2],border_y[y2][x2], x,y ); 
    d3 = chessboard_dist (border_x[y3][x3],border_y[y3][x3], x,y ); 
    d4 = chessboard_dist (border_x[y4][x4],border_y[y4][x4], x,y ); 
#endif


#ifdef EUCLID 
   self = (border_x[y][x] - x) * (border_x[y][x] - x) + 
          (border_y[y][x] - y) * (border_y[y][x] - y); 
#endif
#ifdef CITY_BLOCK 
   self = city_block_dist (border_x[y][x],border_y[y][x], x,y ); 
#endif
#ifdef CHESSBOARD 
   self = chessboard_dist (border_x[y][x],border_y[y][x], x,y ); 
#endif
    dd = min(d1,d2);
    ee = min(d3,d4);
    total = min(dd, ee);
    total = min(self, total); 
  
  if (self <= total)
  { 
    *nadbpx = border_x[y][x]; 
    *nadbpy = border_y[y][x]; 
#ifdef CITY_BLOCK 
    return( self );  
#endif
#ifdef CHESSBOARD 
    return( self );  
#endif
#ifdef EUCLID 
    return( self);  
#endif
  }
  else
  if (total == d1)
  {
    *nadbpx = border_x[y1][x1]; 
    *nadbpy = border_y[y1][x1]; 
#ifdef CITY_BLOCK 
    return( d1 );  
#endif
#ifdef CHESSBOARD 
    return( d1 );  
#endif
#ifdef EUCLID 
    return(  d1 );  
#endif
  }
  else
  if (total == d2)
  {
    *nadbpx = border_x[y2][x2]; 
    *nadbpy = border_y[y2][x2]; 
#ifdef CITY_BLOCK 
    return( d2 );  
#endif
#ifdef CHESSBOARD 
    return( d2 );  
#endif
#ifdef EUCLID 
    return( d2);  
#endif
  }
  else
  if (total == d3)
  {
    *nadbpx = border_x[y3][x3]; 
    *nadbpy = border_y[y3][x3]; 
#ifdef CITY_BLOCK 
    return( d3 );  
#endif
#ifdef CHESSBOARD 
    return( d3 );  
#endif
#ifdef EUCLID 
    return( d3 );  
#endif
  }
  else
  if (total == d4)
  {
    *nadbpx = border_x[y4][x4]; 
    *nadbpy = border_y[y4][x4]; 
#ifdef CITY_BLOCK 
    return( d4 );  
#endif
#ifdef CHESSBOARD 
    return( d4 );  
#endif
#ifdef EUCLID 
    return( d4 ); 
#endif
  }
} /* end bot_euclidean*/
  
/*****************************************************************************
                          TOP_EUCLIDEAN                  
*****************************************************************************/
top_euclidean (y1,x1, y2,x2, y3,x3, y4,x4, nadbpx, nadbpy)
int y1,x1, y2,x2, y3,x3, y4,x4, *nadbpx, *nadbpy;
{
  int x,y, p, p2;
  int d1, d2, d3, d4;
  int dd, ee, total;
  
  x = x2;
  y = y1;
#ifdef EUCLID 
  d1 = (border_x[y1][x1] - x) * (border_x[y1][x1] - x) + 
       (border_y[y1][x1] - y) * (border_y[y1][x1] - y); 
  d2 = (border_x[y2][x2] - x) * (border_x[y2][x2] - x) + 
       (border_y[y2][x2] - y) * (border_y[y2][x2] - y); 
  d3 = (border_x[y3][x3] - x) * (border_x[y3][x3] - x) + 
       (border_y[y3][x3] - y) * (border_y[y3][x3] - y); 
  d4 = (border_x[y4][x4] - x) * (border_x[y4][x4] - x) + 
       (border_y[y4][x4] - y) * (border_y[y4][x4] - y); 
#endif

#ifdef CITY_BLOCK 
    d1 = city_block_dist (border_x[y1][x1],border_y[y1][x1], x,y ); 
    d2 = city_block_dist (border_x[y2][x2],border_y[y2][x2], x,y ); 
    d3 = city_block_dist (border_x[y3][x3],border_y[y3][x3], x,y ); 
    d4 = city_block_dist (border_x[y4][x4],border_y[y4][x4], x,y ); 
#endif
#ifdef CHESSBOARD 
    d1 = chessboard_dist (border_x[y1][x1],border_y[y1][x1], x,y ); 
    d2 = chessboard_dist (border_x[y2][x2],border_y[y2][x2], x,y ); 
    d3 = chessboard_dist (border_x[y3][x3],border_y[y3][x3], x,y ); 
    d4 = chessboard_dist (border_x[y4][x4],border_y[y4][x4], x,y ); 
#endif
    dd = min(d1,d2);
    ee = min(d3,d4);
    total = min(dd,ee);
  
  if (total == d1)
  {
    *nadbpx = border_x[y1][x1]; 
    *nadbpy = border_y[y1][x1]; 
#ifdef CITY_BLOCK 
    return( d1 );  
#endif
#ifdef CHESSBOARD 
    return( d1 );  
#endif
#ifdef EUCLID 
    return( d1 );  
#endif
  }
  else
  if (total == d2)
  {
    *nadbpx = border_x[y2][x2]; 
    *nadbpy = border_y[y2][x2]; 
#ifdef CITY_BLOCK 
    return( d2 );  
#endif
#ifdef CHESSBOARD 
    return( d2 );  
#endif
#ifdef EUCLID 
    return( d2 ); 
#endif
  }
  else
  if (total == d3)
  {
    *nadbpx = border_x[y3][x3]; 
    *nadbpy = border_y[y3][x3]; 
#ifdef CITY_BLOCK 
    return( d3 );  
#endif
#ifdef CHESSBOARD 
    return( d3 );  
#endif
#ifdef EUCLID 
    return( d3);  
#endif
  }
  else
  if (total == d4)
  {
    *nadbpx = border_x[y4][x4]; 
    *nadbpy = border_y[y4][x4]; 
#ifdef CITY_BLOCK 
    return( d4 );  
#endif
#ifdef CHESSBOARD 
    return( d4 );  
#endif
#ifdef EUCLID 
    return( d4 );  
#endif
  }
} /* end top_euclidean  */

/*****************************************************************************
                        PRINT_DIST_TITLE                  
*****************************************************************************/
print_dist_title()
{
#ifdef EUCLID 
  sprintf (strg, "EUCLIDEAN ELEVATION MAP"); 
  alph( strg, 'b', 100, 40 );
#endif
#ifdef CITY_BLOCK 
  sprintf (strg, "CITY BLOCK DISTANCE"); 
  alph( strg, 'b', 114, 60 );
#endif
#ifdef CHESSBOARD 
  sprintf (strg, "CHESSBOARD DISTANCE"); 
  alph( strg, 'b', 114, 60 );
#endif
} /* end function */

/*****************************************************************************
                       DUMP_BORDER       
*****************************************************************************/
dump_border(deltax, minx, maxx, miny, maxy)
 int deltax, minx, maxx, miny, maxy;
{
  int y, j;
  y = 0;
  for (j=miny; j<=maxy; j++)
  {
    VIS$WRITE_HORIZONTAL_LINE (maxx+40, j, deltax, border_x[y]);
    y++;
  }
  y = 0;
  for (j=miny; j<=maxy; j++)
  {
    VIS$WRITE_HORIZONTAL_LINE (maxx+140, j, deltax, border_y[y]);
    y++;
  }
}

/*****************************************************************************
                       DUMP_IMAGE       
*****************************************************************************/
dump_image (deltax, deltay, minx, maxx, miny, maxy)
 int deltax, deltay, minx, maxx, miny, maxy;
{
  int y, i, j, savei=0, savej=0;
  y = 0;
  max_elevation = 0;  
  for (j=1; j <= deltay-2; j++)
  {
   temp_image[j][0] = 0;  
   for (i=1; i<=deltax; i++)
   {
     temp_image[j][i] = round ( sqrt( (float)temp_image[j][i] ) );  
     if (temp_image[j][i] > max_elevation) 
     {   max_elevation = temp_image[j][i];
         savei = i; 
         savej = j; 
     } 
   }
   VIS$WRITE_HORIZONTAL_LINE (minx, miny + j, deltax, temp_image[j]);
  }
  border_x[savej][savei] = 99;
  border_y[savej][savei] = 99;
   
}
/*****************************************************************************
                      DO_NEAR_2_MINS     
*****************************************************************************/
do_near_2_mins (i, j, i1,j1)
int i, j, i1,j1;
{
    int Lij, Lij1, dd;
	
	Lij =    ( (border_x[j][i] - i) * (border_x[j][i] - i) )
	       + ( (border_y[j][i] - j) * (border_y[j][i] - j) );
         
	Lij1 =    ( (border_x[j1][i1] - i) * (border_x[j1][i1] - i) )
	       + (  (border_y[j1][i1] - j) * (border_y[j1][i1] - j) );
	 
    dd = min( Lij, Lij1);
    if (dd == Lij1)
    {
        border_x[j][i] = border_x[j1][i1];
        border_y[j][i] = border_y[j1][i1];
        /*printf ("near 2 mins true \n");  */
    }
} /* end function */

/*****************************************************************************
                       load_inverse_border
*****************************************************************************/
load_inverse_border(x, y, i, pcnum)
 int x,y,i, pcnum;
 {
   piece[pcnum].pathcnt = i;
   piece[pcnum].avgx[i] = (float)x;
   piece[pcnum].avgy[i] = (float)y;
   if (i == 1)
   {
     piece[pcnum].strtx   =  (int)piece[pcnum].avgx[1];
     piece[pcnum].strty   =  (int)piece[pcnum].avgy[1];
   }
} /* end function */

/*****************************************************************************
                       SHOW_INVERSE     
*****************************************************************************/
show_inv (pcnum)
 int pcnum;
 {
   int x, len;
 /*  printf ("border len %d\n", border_len);  
   printf ("path cnt %d\n", piece[pcnum].pathcnt);
*/
   len = piece[pcnum].pathcnt;
   for (x=1;x<=len;x++)
     VIS$SET_PIXEL ( (int)piece[pcnum].avgx[x], (int)piece[pcnum].avgy[x], 255);
 } /* end function */

/*****************************************************************************
                        RWWPAINT_MEDIAL                  
*****************************************************************************/
rwwpaint_medial (deltax, deltay, minx, miny, maxx, maxy)
int deltax, deltay, minx, miny, maxx, maxy;
{
  int i,j,e, z;
  int grey = 1;
  float p; /* double */
  int mat_length=0; 
  float theta;

   sprintf (strg, "This is rwwpaint");  
   printit(strg); 
  waitforkey(); 
}
/*************************************************************************
                        PAINT_MEDIAL                  
*************************************************************************/
paint_medial (deltax, deltay, minx, miny, maxx, maxy)
int deltax, deltay, minx, miny, maxx, maxy;
{
  int i,j,e, z;
  int grey = 1;
  int mat_length=0; 
  
  sprintf (strg, "in paint medial");
  printit(strg); 
  waitforkey();

  
  j = deltay / 2; 
  VIS$SELECT_LUT (1,0); 
  for (i=20; i<=deltax; i++)
  { 
    if (image[j][i] != 0) 
    {
       if ( local_maxima (j, i, 4))
       {
          printf ("yes it is a local max \n");
          VIS$SET_PIXEL ( i + minx, j + miny, 255);
	  printf ("x: %d y: %d \n", i+minx, j+miny);
	  mat_length = 0; 
          /*medial_mode();  */
	  follow(j,i, minx, miny, &mat_length); 
	  printf ("mat_length: %d \n", mat_length);
	  if (mat_length > MEDIAL_LNGTH_THRESH)
	     return (mat_length); 
	  return (mat_length); 
       }
     }/* endif*/
   } /* end for i*/
   return (0); 
 } /* end */

/*-------------------------------- localmaxima -------------------------------*/
/* checks all 4-8 points around the input pixel and determines if there are
    any neighboring pixels with a greater grey shade. */
/*-------------------------------- localmaxima -------------------------------*/
local_maxima (x,y, connected)
 int x,y, connected;
{
  int i, j, grey;
  grey = temp_image[y][x]; 
  
  if ( (temp_image[y][x+1] < grey) || (temp_image[y][x-1] < grey) ||
     (temp_image[y+1][x] < grey) || (temp_image[y-1][x] < grey) )
      return(NO);
  if (connected == 8)
    if ( (temp_image[y+1][x+1] < grey) || (temp_image[y-1][x+1] < grey) ||
         (temp_image[y+1][x-1] < grey) || (temp_image[y-1][x-1] < grey) )
        return(NO);
  return(YES);
}

medial_mode()
{
  int i, mode_vals[256];
   
   VIS$SET_MODE(1);
   mode_vals[0] = 0;
   if (inverse_isthmus)
      mode_vals[255] = 254;
   else
      mode_vals[255] = 255;
   mode_vals[254] = 254;
   for(i=1;i<254;i++)
      mode_vals[i] = 254;
   VIS$LOAD_LUT(0, 4, mode_vals);
   sleep(1);
   VIS$FREEZE(); 
   VIS$SET_MODE(0);
}

/*****************************************************************************
                             FOLLOW                        
*****************************************************************************/
follow (j,i,minx, miny, mat_length)
int j,i, minx, miny, *mat_length;
{
  int x,y,z, grey;
  VIS$SET_PIXEL (i + minx, j + miny, 255);
  printf ("following pixel at i %d j %d \n", i + minx, j + miny);  
  printf ("i is %d j is %d \n", i, j);  
  printf ("value was %d: \n", temp_image[j][i]);  

  for (z=1;z<=8;z++)
  {
      get_next_neighbor (z, i, j, &x, &y);
      grey = VIS$GET_PIXEL (x + minx, y + miny);
      if (grey != 255  && local_maxima (y,x, 4))
      {
	*mat_length++;
        follow(y,x, minx, miny, mat_length);
      }
  } /* end for z */ 
}  /* end function */

/*****************************************************************************
                              END                     
*****************************************************************************/