// Converter from xfig to LEDA/C++ by Libo Yang

#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <iostream.h>
#include <string.h>

#include <LEDA/polygon.h>
#include <LEDA/list.h>
#include <LEDA/point.h>
#include <LEDA/window.h>
#include <LEDA/ps_file.h>

#define BUFSIZE 100
#define GRIDSIZE 100
#define PAGE_W 16.0
#define PAGE_H 23.5
#define STEP_SIZE 5
#define STEP_NUMBER 4000
#define BOUNDRY_COLOR -1
#define OBSTACLE_COLOR 43
#define ROBOT_INIT 20
#define ROBOT_GOAL 30
#define DELT_X 1
#define DELT_Y 1
#define DELT_STA 0.062831853
#define PI 3.1415926
#define F_MAX 100
#define F_MIN 0.1
#define T_MAX 1.0e5

char xfig_file[100];
char out_root[100];
FILE *ifp;
char buf[BUFSIZE];
list <polygon> rv[4]; // rv[0] is boundary; rv[1] is obstacle; rv[2,3] is robot
point origin;
int win_x=600;
int win_y=600;
double x_max,y_max;
double A_B[3000000];
long AB_offset = 0;
struct AliveBand
  {
    double TT;
    int i;
    int j;
    int k;
  } M_B[10000];
long length_MB = 0;
struct NarrowBand
  {
    double TT;
    double FF;
    int i;
    int j;
    int k;
  } N_B[3000000];
long length_NB = 0;
int i_x = 0,i_y = 0,i_sta = 0;
int g_x,g_y,g_sta;
//int g_x = 73,g_y = 0 ,g_sta = 0;


// This file convers XFIG file to LEDA/C++ 
void xfig_poly()
{
  int i;
  float p1,p2;
  float num=1;
  int color;
  list <point> plist;
  polygon *pl;
  point *pi;

  for(i=1; i<10; i++)
    {
      fgets(buf,BUFSIZE,ifp);
    }

  while(fgets(buf,BUFSIZE,ifp) != NULL)
    {
      sscanf(buf,"%f %f %f %f %f %f %f %f %d %f %f %f %f %f %f %f",&num,&num,&num,&num,&num,&num,&num,&num,&color,&num,&num,&num,&num,&num,&num,&num);
      for(i=1; i<=(int)num-1; i++)
	{
	  fscanf(ifp,"%f %f",&p1,&p2);
	  pi = new point(p1,p2);
	  plist.append(*pi);
	}
      // cout << plist << "\n";
      pl = new polygon(plist);
      switch(color)
	{
	  case BOUNDRY_COLOR: rv[0].append(*pl);
	    break;
	  case ROBOT_INIT:  rv[2].append(*pl);
	    break;
	  case ROBOT_GOAL:  rv[3].append(*pl);
	    break;
	    //default:  rv[0].append(*pl);
	    //break;
	  default: rv[1].append(*pl);
	    break;
	}     
      plist.clear();
      fscanf(ifp,"%f",&num);
      fscanf(ifp,"%f",&num);
      fgets(buf,BUFSIZE,ifp);
    }
}  

// This file uniforms the graph
void uniform_poly()
{

  int i;
  polygon poly,*npoly;
  list <polygon> new_poly;
  list <point> pi;
  point pt, *npt;
  double new_x,new_y,scale;
  list_item it;

  poly = rv[0].front();
  pi = poly.vertices();
  origin = pi.back();
  
  it = pi.get_item(2);
  pt = pi.contents(it);

  x_max = pt.xcoord();
  y_max = pt.ycoord();
  
  if(pt.xcoord() > pt.ycoord())
    {
       scale = GRIDSIZE/pt.xcoord();
       win_y = (int)(win_x*(pt.ycoord()/pt.xcoord()))+10;
    }
  else
    {
       scale = GRIDSIZE/pt.ycoord();
       win_x = (int)(win_y*(pt.xcoord()/pt.ycoord()))+10;
    }

  for(i=0; i<=3; i++)
    {
      new_poly.clear();
      forall(poly,rv[i])
	{
	  pi.clear();
	  forall_vertices(pt,poly)
	    {
	      new_x = (pt.xcoord() - origin.xcoord())*scale;
	      new_y = (origin.ycoord() - pt.ycoord())*scale;
	      npt = new point(new_x,new_y);
	      pi.push(*npt);
	    }
	  npoly = new polygon(pi);
	  new_poly.append(*npoly);
	}
      rv[i] = new_poly;  
      cout << rv[i] << endl;
    }

  x_max = (x_max - origin.xcoord())*scale;
  y_max = (origin.ycoord() - y_max)*scale;
  
}


// if collides, return TRUE; otherwise return FALSE
bool collision_detection( polygon current_robot )
{
  polygon poly;
  list <polygon> result_listpoly;
  bool s = 1;

  forall(poly,rv[1])
    {
      result_listpoly = poly.intersection(current_robot);
      s = s && (result_listpoly.empty());
    }
  
  s = !s;

  poly = rv[0].front();
  result_listpoly = poly.intersection(current_robot);
  poly = result_listpoly.front();
  if (poly.area() == current_robot.area())
    {
      s = s || 0;
    }
  else
    {
      s = s || 1;
    }

  return s;
}

void grasp_goal()
{
  polygon poly;
  list <point> pi;
  point pt;
  poly = rv[2].front();
  pi = poly.vertices();
  pt = pi.front();

  int i,j;
  i = (int)(pt.xcoord());
  j = (int)(pt.ycoord());

  AB_offset = i*10000 + j*100 + 50 + 1000000;

  cout << "AB_offset = " << AB_offset << endl;;

  poly = rv[3].front();
  pi = poly.vertices();
  pt = pi.front();

  int i2,j2;
  i2 = (int)(pt.xcoord());
  j2 = (int)(pt.ycoord());

  g_x = (int)((i2 - i)/DELT_X);
  g_y = (int)((j2 - j)/DELT_Y);
  g_sta = 0;

  cout << "goal :(" << g_x << "," << g_y << "," << g_sta << ")" << endl;

}

polygon oritation_robot(double x, double y, double sta)
{
  polygon poly;
  point pt,*c_p; // c_p is reference point for rotation. It is definded as the center of the robot
  double c_x = 0,c_y = 0;
  int i = 0;

  poly = rv[2].front();
  
  poly = poly.translate(x,y);

  forall_vertices(pt,poly)
    {
      c_x = c_x + pt.xcoord();
      c_y = c_y + pt.ycoord();
      i = i + 1;
    }
  c_x = c_x/i;
  c_y = c_y/i;
  c_p = new point(c_x,c_y);

  poly = poly.rotate((*c_p),sta);  

  return poly;
}

double F(int i, int j, int k)
{
  double x,y,sta;
  polygon robot;

  x = (i - i_x) * DELT_X;
  y = (j - i_y) * DELT_Y;
  sta = (k - i_sta) * DELT_STA;

  if ((sta >= PI) || (sta <= ((-1)*PI)))
    {
      return F_MIN;
    }

  robot = oritation_robot(x,y,sta);

  if( collision_detection(robot))
    {
      return F_MIN;
    }
  else
    {
      return F_MAX;
    }

}

double S_AB(int i,int j,int k)
{
  double t;

  if((i == 0) && (j == 0) && (k == 0))
    {
      return 0;
    }

  t = A_B[i*10000 + j*100 + k + AB_offset];
  if( t > 0 )
    {
      return t;
    }

  return T_MAX;

}


void push_MB(double v, int i, int j, int k)
{
  M_B[length_MB].TT = v;
  M_B[length_MB].i  = i;
  M_B[length_MB].j  = j;
  M_B[length_MB].k  = k;

  length_MB = length_MB + 1;

}

void push_NB(double v,double f,int i,int j,int k)
{
  long m,n;

  if( length_NB == 0 )
    {
      N_B[0].TT = v;
      N_B[0].FF = f;
      N_B[0].i = i;
      N_B[0].j = j;
      N_B[0].k = k;
      length_NB = 1;
    }
  else
    {
      for( m=0; m < length_NB; m++)
	{
	  if(v <= N_B[m].TT)
	    {
	      for(n=length_NB ; n > m; n--)
		{
		  N_B[n] = N_B[n - 1];
		}
	      N_B[m].TT = v;
	      N_B[m].FF = f;
	      N_B[m].i = i;
	      N_B[m].j = j;
	      N_B[m].k = k;
	      length_NB = length_NB + 1;
	      return;
	    }
	}
      N_B[length_NB].TT = v;
      N_B[length_NB].FF = f;
      N_B[length_NB].i = i;
      N_B[length_NB].j = j;
      N_B[length_NB].k = k;
      length_NB = length_NB +1;
    }
  return;
}


NarrowBand pop_NB()
{
  long i;
  NarrowBand temp;

  temp = N_B[0];

  for(i=1; i<length_NB;i++)
    {
      N_B[i - 1] = N_B[i];
    }
  length_NB = length_NB - 1;

  A_B[temp.i*10000 + temp.j*100 + temp.k + AB_offset] = temp.TT;

  return temp;
}

double SF_NB(int i,int j,int k)
{
  long n;
  
  for(n=0; n<length_NB; n++)
    {
      if( (N_B[n].i == i) && (N_B[n].j == j) && (N_B[n].k == k))
	{
	  return N_B[n].FF;
	}
    }
}

void renew_NB(double v,int i,int j,int k)
{
  long n,m = 0;
  double f = 0;
  
  for(n=0; n<length_NB; n++)
    {
      if( (N_B[n].i == i) && (N_B[n].j == j) && (N_B[n].k == k))
	{
	  f = N_B[n].FF;
	  m = n;
	}
    }

  for(n= m + 1; n<length_NB; n++)
    {
      N_B[n - 1] = N_B[n];
    }
  length_NB = length_NB - 1;

  push_NB(v,f,i,j,k);

}

double T(int i, int j, int k)
{
  long n;
  double t;

  if((i == 0) && (j == 0) && (k == 0))
    {
      return 0;
    }

  t = A_B[i*10000 + j*100 + k + AB_offset];
  if( t > 0 )
    {
      return t;
    }

  for(n=(length_NB - 1); n >= 0; n--)
    {
      if( (N_B[n].i == i) && (N_B[n].j == j) && (N_B[n].k == k))
	{
	  return N_B[n].TT;
	}
    }

  return T_MAX;
}

void initilization()
{
  int i;
  double f;
 
  // define alive points
  //push_AB(0,i_x,i_y,i_sta);
  A_B[i_x*10000 + i_y*100 + i_sta + AB_offset] = 0;

  // define narrow band points and push them into N_B
  f = F(i_x + 1,i_y,i_sta); 
  push_NB(DELT_X/f ,f, i_x + 1 , i_y , i_sta );

  f = F(i_x - 1,i_y,i_sta);  
  push_NB(DELT_X/f ,f, i_x - 1 , i_y , i_sta );

  f = F(i_x,i_y + 1,i_sta);
  push_NB(DELT_Y/f ,f, i_x , i_y + 1 , i_sta );

  f = F(i_x,i_y - 1,i_sta);
  push_NB(DELT_Y/f ,f, i_x , i_y - 1 , i_sta );

  f = F(i_x,i_y,i_sta + 1);
  push_NB(DELT_STA/f ,f, i_x , i_y , i_sta + 1 );

  f = F(i_x,i_y,i_sta - 1);
  push_NB(DELT_STA/f ,f, i_x , i_y , i_sta - 1 );

  cout << " T[" << i_x << "," << i_y << "," << i_sta << "] = " << T(i_x,i_y,i_sta) << "\n\n";
  for( i=0; i<length_NB ; i++)
    {
      cout << " T[" << N_B[i].i << "," << N_B[i].j << "," << N_B[i].k << "] = " << N_B[i].TT << " = " << T(N_B[i].i,N_B[i].j,N_B[i].k) << "\n";
    }
}

double max(double a, double b)
{
  if( a >= b)
    {
      return a;
    }
  else
    {
      return b;
    }
}

double min(double a, double b)
{
  if( a <= b)
    {
      return a;
    }
  else
    {
      return b;
    }
}

double EQ(double x, double f, double T1, double T2, double T3, double T4, double T5, double T6)
{
  double a1,a2,a3,a4,a5,a6,a;

  a1 = max(((x - T1)/DELT_X),0);
  a2 = min(((T2 - x)/DELT_X),0);
  a3 = max(((x - T3)/DELT_Y),0);
  a4 = min(((T4 - x)/DELT_Y),0);
  a5 = max(((x - T5)/DELT_STA),0);
  a6 = min(((T6 - x)/DELT_STA),0);

  a = a1*a1 + a2*a2 + a3*a3 + a4*a4 + a5*a5 + a6*a6 - 1/(f*f);

  //cout << "------- x = " << x << " F = " << f <<"("<< i <<","<< j <<","<<k<<")"<<endl;
  //cout << "a1 = "  << a1 << endl;
  //cout << "a2 = "  << a2 << endl;
  //cout << "a3 = "  << a3 << endl;
  //cout << "a4 = "  << a4 << endl;
  //cout << "a5 = "  << a5 << endl;
  //cout << "a6 = "  << a6 << endl;
  //cout << " EQ = " << a << endl;

  return a;
}


double T_NEW(double T_min,double f,int i,int j, int k)
{
  long n;
  double x1,x2,EQ_T2,EQ_x1;
  double T1,T2,T3,T4,T5,T6;

  T1 = T(i - 1,j,k);
  T2 = T(i + 1,j,k);
  T3 = T(i,j - 1,k);
  T4 = T(i,j + 1,k);  
  T5 = T(i,j,k - 1); 
  T6 = T(i,j,k + 1); 

  x2 = T(i,j,k);
  EQ_T2 = EQ(T_min,f,T1,T2,T3,T4,T5,T6);

  for(n=0; n<=1000000; n++)
    {
      x1 = (T_min + x2)/2;
      EQ_x1 = EQ(x1,f,T1,T2,T3,T4,T5,T6);
      
      if((EQ_x1*EQ_T2) >= 0)
	{
	  T_min = x1;
	  EQ_T2 = EQ_x1;
	}
      else
	{
	  x2 = x1;
	}

     if( (EQ_x1 < 1.0e-8) && (EQ_x1 > -1.0e-8))
	{
	  return x1;
	}
    }
}


void fast_marching()
{
  NarrowBand NB_min;
  double t,f;
  long n;
  int i,j,k,l;

  for(n=0; n<1000000000; n++)
    {
// pop up the smallest point in the narrow band, and add it into alive band
      NB_min = pop_NB();
      i = NB_min.i;
      j = NB_min.j;
      k = NB_min.k;

       cout << " AB[" << i << "," << j << "," << k << "] = " <<  A_B[i*10000 + j*100 + k + AB_offset] << "\n";


// Detect if reaches the goal

      if((i == g_x) && (j == g_y) &&(k == g_sta))
	{
	  return;
	}

// renew the points surrunding this point

      ////////////////////////////
      // step 1: renew (i - 1,j,k)
      ///////////////////////////

      t = T(i - 1,j,k);
      
      if( t > NB_min.TT)
	{
	  if(t < T_MAX)
	    {
	      f = SF_NB(i - 1,j,k);
	      renew_NB(T_NEW(NB_min.TT,f,i - 1,j,k),i - 1,j,k);
	    }
	  else
	    {
	      f = F(i - 1,j,k);
	      push_NB(T_NEW(NB_min.TT,f,i - 1,j,k),f,i - 1,j,k);
	    }
	  /*
	  cout << "-------------------------" << "\n";
	  cout << "F = " << f << endl;
	  cout << " T[" << i - 1 << "," << j << "," << k << "] = " << T(i - 1,j,k) << "\n\n";
	  for(l=0;l<length_NB;l++)
	    {
	      cout << " NB[" << N_B[l].i << "," << N_B[l].j << "," << N_B[l].k << "] = " <<  N_B[l].TT << "\n";
	    }
	  */
	}
            
      ////////////////////////////////////
      // step 2: renew (i + 1,j,k)
      ////////////////////////////////////

      t = T(i + 1,j,k);
      
      if( t > NB_min.TT)
	{
	  if(t < T_MAX)
	    {
	      f = SF_NB(i + 1,j,k);
	      renew_NB(T_NEW(NB_min.TT,f,i + 1,j,k),i + 1,j,k);
	    }
	  else
	    {
	      f = F(i + 1,j,k);
	      push_NB(T_NEW(NB_min.TT,f,i + 1,j,k),f,i + 1,j,k);
	    }
	  /*
	  cout << "-------------------------" << "\n";
	  cout << "F = " << f << endl;
	  cout << " T[" << i + 1 << "," << j << "," << k << "] = " << T(i + 1,j,k) << "\n\n";
	  for(l=0;l<length_NB;l++)
	    {
	      cout << " NB[" << N_B[l].i << "," << N_B[l].j << "," << N_B[l].k << "] = " <<  N_B[l].TT << "\n";
	    }
	  */
	}

      ///////////////////////////////////
      // step 3: renew (i,j - 1,k)
      ////////////////////////////////////
      
      t = T(i,j - 1,k);
      
      if( t > NB_min.TT)
	{
	  if(t < T_MAX)
	    {
	      f = SF_NB(i,j - 1,k);
	      renew_NB(T_NEW(NB_min.TT,f,i,j - 1,k),i,j - 1,k);
	    }
	  else
	    {
	      f = F(i,j - 1,k);
	      push_NB(T_NEW(NB_min.TT,f,i,j - 1,k),f,i,j - 1,k);
	    }
	  /*
	  cout << "-------------------------" << "\n";
	  cout << "F = " << f << endl;
	  cout << " T[" << i << "," << j - 1 << "," << k << "] = " << T(i,j - 1,k) << "\n\n";
	  for(l=0;l<length_NB;l++)
	    {
	      cout << " NB[" << N_B[l].i << "," << N_B[l].j << "," << N_B[l].k << "] = " <<  N_B[l].TT << "\n";
	    }
	  */
	}

      ///////////////////////////////////
      // step 4: renew (i,j + 1,k)
      ////////////////////////////////////
      
      t = T(i,j + 1,k);
      
      if( t > NB_min.TT)
	{
	  if(t < T_MAX)
	    {
	      f = SF_NB(i,j + 1,k);
	      renew_NB(T_NEW(NB_min.TT,f,i,j + 1,k),i,j + 1,k);
	    }
	  else
	    {
	      f = F(i,j + 1,k);
	      push_NB(T_NEW(NB_min.TT,f,i,j + 1,k),f,i,j + 1,k);
	    }
	  /*
	  cout << "-------------------------" << "\n";
	  cout << "F = " << f << endl;
	  cout << " T[" << i << "," << j + 1 << "," << k << "] = " << T(i,j + 1,k) << "\n\n";
	  for(l=0;l<length_NB;l++)
	    {
	      cout << " NB[" << N_B[l].i << "," << N_B[l].j << "," << N_B[l].k << "] = " <<  N_B[l].TT << "\n";
	    }
	  */
	}

      ///////////////////////////////////
      // step 5: renew (i,j,k - 1)
      ////////////////////////////////////
      
      t = T(i,j,k - 1);
      
      if( t > NB_min.TT)
	{
	  if(t < T_MAX)
	    {
	      f = SF_NB(i,j,k - 1);
	      renew_NB(T_NEW(NB_min.TT,f,i,j,k - 1),i,j,k - 1);
	    }
	  else
	    {
	      f = F(i,j,k - 1);
	      push_NB(T_NEW(NB_min.TT,f,i,j,k - 1),f,i,j,k - 1);
	    }
	  /*
	  cout << "-------------------------" << "\n";
	  cout << "F = " << f << endl;
	  cout << " T[" << i << "," << j << "," << k - 1 << "] = " << T(i,j,k - 1) << "\n\n";
	  for(l=0;l<length_NB;l++)
	    {
	      cout << " NB[" << N_B[l].i << "," << N_B[l].j << "," << N_B[l].k << "] = " <<  N_B[l].TT << "\n";
	    }
	  */
	}


      ///////////////////////////////////
      // step 6: renew (i,j,k + 1)
      ////////////////////////////////////
      
      t = T(i,j,k + 1);
      
      if( t > NB_min.TT)
	{
	  if(t < T_MAX)
	    {
	      f = SF_NB(i,j,k + 1);
	      renew_NB(T_NEW(NB_min.TT,f,i,j,k + 1),i,j,k + 1);
	    }
	  else
	    {
	      f = F(i,j,k + 1);
	      push_NB(T_NEW(NB_min.TT,f,i,j,k + 1),f,i,j,k + 1);
	    }
	  /*
	  cout << "-------------------------" << "\n";
	  cout << "F = " << f << endl;
	  cout << " T[" << i << "," << j << "," << k + 1 << "] = " << T(i,j,k + 1) << "\n\n";
	  for(l=0;l<length_NB;l++)
	    {
	      cout << " NB[" << N_B[l].i << "," << N_B[l].j << "," << N_B[l].k << "] = " <<  N_B[l].TT << "\n";
	    }
	  */
	}           

    }

}

void feed_back()
{
  int i,j,k,i2,j2,k2,l;
  long n;
  double t,t1,t2,t3,grad;

  M_B[0].TT = A_B[g_x*10000 + g_y*100 + g_sta + AB_offset];
  M_B[0].i = g_x;
  M_B[0].j = g_y;
  M_B[0].k = g_sta;

  //M_B[0] = A_B[length_AB - 1];
  length_MB = 1;
  i = M_B[0].i;
  j = M_B[0].j;
  k = M_B[0].k;
  t1 = M_B[0].TT;

  for(n=0; n< 1000000000; n++)
    {
      if((i == i_x) && (j == i_y) &&(k == i_sta))
	{
	  cout <<" !!!!!!!!!!!!!" << endl;

	  for(l=0;l<length_MB;l++)
	    {
	      cout << " MB[" << M_B[l].i << "," << M_B[l].j << "," << M_B[l].k << "] = " <<  M_B[l].TT << "\n";
	    }    
	  return;
	}

      t2 = S_AB(i - 1,j,k);
      t = t2;
      grad = (t1 - t2)/DELT_X;
      i2 = i - 1;
      j2 = j;
      k2 = k;

      t2 = S_AB(i + 1,j,k);
      t3 = (t1 - t2)/DELT_X;
      if(t3 > grad)
	{
	  t = t2;
	  grad = t3;
	  i2 = i + 1;
	  j2 = j;
	  k2 = k;
	}

      t2 = S_AB(i,j - 1,k);
      t3 = (t1 - t2)/DELT_Y;
      if(t3 > grad)
	{
	  t = t2;
	  grad = t3;
	  i2 = i;
	  j2 = j - 1;
	  k2 = k;
	}

      t2 = S_AB(i,j + 1,k);
      t3 = (t1 - t2)/DELT_Y;
      if(t3 > grad)
	{
	  t = t2;
	  grad = t3;
	  i2 = i;
	  j2 = j + 1;
	  k2 = k;
	}

      t2 = S_AB(i,j,k - 1);
      t3 = (t1 - t2)/DELT_STA;
      if(t3 > grad)
	{
	  t = t2;
	  grad = t3;
	  i2 = i;
	  j2 = j;
	  k2 = k - 1;
	}

      t2 = S_AB(i,j,k + 1);
      t3 = (t1 - t2)/DELT_STA;
      if(t3 > grad)
	{
	  t = t2;
	  grad = t3;
	  i2 = i;
	  j2 = j;
	  k2 = k + 1;
	}

      push_MB(grad,i2,j2,k2);

      i = i2;
      j = j2;
      k = k2;
      t1 = t;
    }

}

// This file shows the original graph
void display()
{
  long i;
  int j;
  polygon poly,*npoly;
  list <polygon> new_poly;
  list <point> pi;
  point pt,*npt;
  double new_x,new_y;
  char out_file[100];
  char p_1[3];
  char p_2[4];
  
  p_1[0] = '.';
  p_1[1] = 'p';
  p_1[2] = 's';

  strcpy(out_file,out_root);
  strncat(out_file,p_1,3);

  window w(win_x,win_y);
  ps_file P(PAGE_W,PAGE_H,out_file);
  w.open(window::center,window::center);
  w.init(0,GRIDSIZE,0);
  P.init(0,GRIDSIZE,0);
  
  for(i=0;i<=3;i++)
    {
      new_poly.clear();
      forall(poly,rv[i])
	{
	  pi.clear();
	  forall_vertices(pt,poly)
	    {
	      new_x = pt.xcoord() + GRIDSIZE/100;
	      new_y = pt.ycoord() + GRIDSIZE/100;
	      npt = new point(new_x,new_y);
	      pi.push(*npt);
	    }
	  npoly = new polygon(pi);
	  new_poly.append(*npoly);
	}  
      switch(i)
	 {
	 case 0: 
	   poly = new_poly.pop();
	   w.draw_polygon(poly,black);
	   P.draw_polygon(poly,black);
	   break;
	 case 1:
	   forall(poly,new_poly)
	     {
	       w.draw_filled_polygon(poly,black);
	       P.draw_filled_polygon(poly,black);
	     }
	   break;
	 case 2: 
	   poly = new_poly.pop();
	   w.draw_filled_polygon(poly,red);
	   P.draw_filled_polygon(poly,red);
	   break;
	 }
    }
  
  // to draw a moving robot

  for(i=(length_MB - 1); i>= 0; i = i - 2)
    {
      double x,y,sta;

      x = (M_B[i].i - i_x)*DELT_X;
      y = (M_B[i].j - i_y)*DELT_Y;
      sta = (M_B[i].k - i_sta)*DELT_STA;

      poly = oritation_robot(x,y,sta);
      pi.clear();
      forall_vertices(pt,poly)
	{
	  new_x = pt.xcoord() + GRIDSIZE/100;
	  new_y = pt.ycoord() + GRIDSIZE/100;
	  npt = new point(new_x,new_y);
	  pi.push(*npt);
	}
      npoly = new polygon(pi);
  
      //w.draw_filled_polygon(*npoly,black);
      //P.draw_filled_polygon(*npoly,black);

      w.draw_polygon(*npoly,red);
      P.draw_polygon(*npoly,red);

    }

  // to draw a series of .ps files

  p_2[0] = '0';
  p_2[1] = '0';
  p_2[2] = '0';
  p_2[3] = '0';

  int d1,d2,d3,d4;
  polygon f_poly;

  for(j=0;j<2;j++)
    {
      new_poly.clear();
      forall(poly,rv[j])
	{
	  pi.clear();
	  forall_vertices(pt,poly)
	    {
	      new_x = pt.xcoord() + GRIDSIZE/100;
	      new_y = pt.ycoord() + GRIDSIZE/100;
	      npt = new point(new_x,new_y);
	      pi.push(*npt);
	    }
	  npoly = new polygon(pi);
	  new_poly.append(*npoly);
	}  
      if(j == 0) 
	{
	   f_poly = new_poly.pop();
	}
    }

  for( i=0;i<length_MB;i++)
    {
      if (i<10)
	{
	  p_2[3] = 48+i;
	}
      else
	{
	  if (i < 100)
	    {
	      d3 = i/10;
	      d4 = i - d3*10;
	      p_2[3] = 48 + d4;
	      p_2[2] = 48 + d3;
	    }
	  else
	    {
	      if(i < 1000)
		{
		  d2 = i/100;
		  d3 = (i - d2*100)/10;
		  d4 = i - d2*100 - d3*10;
		  p_2[3] = 48 + d4;
		  p_2[2] = 48 + d3;
		  p_2[1] = 48 + d2;
		}
	      else
		{
		  d1 = i/1000;
		  d2 = (i- d1*1000)/100;
		  d3 = (i - d1*1000 - d2*100)/10;
		  d4 = i - d1*1000 - d2*100 - d3*10;
		  p_2[3] = 48 + d4;
		  p_2[2] = 48 + d3;
		  p_2[1] = 48 + d2;
		  p_2[0] = 48 + d1;
		}
	    }
	}
      strcpy(out_file,out_root);
      strncat(out_file,p_2,4);
      strncat(out_file,p_1,3);

      ps_file P(PAGE_W,PAGE_H,out_file);
      P.init(0,GRIDSIZE,0);
  
      for(j=0;j<2;j++)
	{
	  switch(j)
	    {
	    case 0: 
	      P.draw_polygon(f_poly,black);
	      break;
	    case 1:
	      forall(poly,new_poly)
		{
		  P.draw_filled_polygon(poly,black);
		}
	      break;
	    }
	}

      double x,y,sta;

      x = (M_B[i].i - i_x)*DELT_X;
      y = (M_B[i].j - i_y)*DELT_Y;
      sta = (M_B[i].k - i_sta)*DELT_STA;

      poly = oritation_robot(x,y,sta);
      pi.clear();
      forall_vertices(pt,poly)
	{
	  new_x = pt.xcoord() + GRIDSIZE/100;
	  new_y = pt.ycoord() + GRIDSIZE/100;
	  npt = new point(new_x,new_y);
	  pi.push(*npt);
	}
      npoly = new polygon(pi);
  
      P.draw_filled_polygon(*npoly,red);     
    }

  while(!w.read_mouse());

}

main()
{  
  cout << "Please input xfig file name:  ";
  cin >> xfig_file;
  cout << "Type output ps_file name(*.ps):  ";
  cin >> out_root;

  ifp = fopen(xfig_file, "r");
  if( !ifp )
    {
      cerr << " XFIG File Open Failed \n";
      exit(-1);
    }
  
  xfig_poly();
  uniform_poly();
  grasp_goal();
  initilization();
  fast_marching();
  feed_back();

  display();
    
  fclose(ifp);

}