/***************************/
/* Path Finder V1.0 1997   */
/* for Java API 1.1.3      */ 
/* Qing Li <llysc@mit.edu> */
/***************************/

import java.applet.*;
import java.awt.*;
import java.awt.event.*;
import java.net.*;
import java.util.*;

public class Path extends Applet implements Runnable
{
    // image for double-buffering.
    Image backBuffer,newbackBuffer;
     // graphics context of double-buffering.
    Graphics backBufferG,newbackBufferG; 
    // overload interface Runnable's run()
    Thread animator;
    int num_obstacle = 3;
    int obstacle_size = 40;
    int obstacle_edges = 4;
    int num_gem = 3;
    int gem_radius = 10;
    int rover_width = 30;
    int rover_height = 20;
    int rover_angle = 0;
    int delay = 10;
    double tmp;
    int p_inter , tmp_n;

    Vector myScene = new Vector();
    Vector myPath = new Vector();

    Vector inter_p = new Vector(); // tmp point bw oringin and target points
    Point p1 = new Point(0,0);  // origin point
    Point p2 = new Point(0,0);  // target point

    Vector obs = new Vector();

    // create "more than enough" empty objects -- use "vector" later.
    //Vector dressed_obstacles = new Vector();
    //D_Obstacle D_obs = new D_Obstacle();
    
    D_Obstacle[] dressed_obstacles = {
	new D_Obstacle(), new D_Obstacle(), new D_Obstacle(),
	new D_Obstacle(), new D_Obstacle(), new D_Obstacle(),
	new D_Obstacle(), new D_Obstacle(), new D_Obstacle(),
	new D_Obstacle(), new D_Obstacle(), new D_Obstacle(),
	new D_Obstacle(), new D_Obstacle(), new D_Obstacle(),
	new D_Obstacle(), new D_Obstacle(), new D_Obstacle(),
	new D_Obstacle(), new D_Obstacle(), new D_Obstacle(),
    };

    Vector gems = new Vector();
    //D_Gem D_gem = new D_Gem();
    
    D_Gem[] dressed_gems = {
	new D_Gem(), new D_Gem(), new D_Gem(), new D_Gem(),
	new D_Gem(), new D_Gem(), new D_Gem(), new D_Gem(),
	new D_Gem(), new D_Gem(), new D_Gem(), new D_Gem(),
	new D_Gem(), new D_Gem(), new D_Gem(), new D_Gem(),
	new D_Gem(), new D_Gem(), new D_Gem(), new D_Gem(),
	new D_Gem(), new D_Gem(), new D_Gem(), new D_Gem()
    };
    
    Rover rover = new Rover(this);
    
    public void init()
    {
	String arg;
	int i, j, k, x, y;
	boolean success;
	
	// get parameters from applet <param> tag fields
	arg = getParameter("num_obstacle");
	if (arg != null) num_obstacle = Integer.parseInt(arg);
	arg = getParameter("obstacle_size");
	if (arg != null) obstacle_size = Integer.parseInt(arg);
	arg = getParameter("obstacle_edges");
	if (arg != null) obstacle_edges = Integer.parseInt(arg);
	arg = getParameter("num_gem");
	if (arg != null) num_gem = Integer.parseInt(arg);
	arg = getParameter("gem_radius");
	if (arg != null) gem_radius = Integer.parseInt(arg);

	// Create double buffer with getSize of the animation box
	backBuffer = createImage(512+1,512+1);
	backBufferG = backBuffer.getGraphics();
	
	// create random obstacles
	int xpoints[] = new int [obstacle_edges];
	int ypoints[] = new int [obstacle_edges];
	double angle[] = new double [obstacle_edges];
	double alpha = 0.5;
	for (i=0; i<num_obstacle; i++)
	{
	
	    success = false;
	    while (!success)
	    {
		success = true;
		x = Boundary.minx + (int) Math.round((Boundary.maxx - Boundary.minx) * Math.random());
		y = Boundary.miny + (int) Math.round((Boundary.maxy - Boundary.miny) * Math.random());
		// generate random convex polygon touching a circle 
		for (j=0; j<obstacle_edges; j++)
		{
		    angle[j] = 2 * Math.PI * Math.random();
		    // sort the angles
		    for (k=j-1; k>=0; k--)
		    {
			if (angle[k+1] < angle[k])
			{
			    // swap the two angles
			    tmp = angle[k];
			    angle[k] = angle[k+1];
			    angle[k+1] = tmp;
			}
		    }
		}
		for (j=0; j<obstacle_edges; j++)
		    angle[j] = angle[j]*alpha + (1-alpha)* 2 * Math.PI * (j-1)/obstacle_edges;
		
		for (j=0; j<obstacle_edges; j++)
		{
		    xpoints[j] = x + (int) Math.round(obstacle_size * Math.cos(angle[j])/2);
		    ypoints[j] = y + (int) Math.round(obstacle_size * Math.sin(angle[j])/2);
		    success = success && Boundary.contains(xpoints[j], ypoints[j]);
		    
		}
		if (success)
		{
		    Polygon obstacle = new Polygon(xpoints, ypoints, obstacle_edges);
		    // Must doesn't touch previous obstacles
		    for (j=0; j<i; j++)
			success = success && (!Rover.overlap(dressed_obstacles[j].body, obstacle));
		    // dress the obstacle
		    /*
		    if(success)
			{
			    D_obs.dress(obstacle, rover_width, rover_height);
			    dressed_obstacles.addElement(D_obs);
			}*/
			
		    if (success)
			{
			    dressed_obstacles[i].dress (obstacle, rover_width, rover_height);
			    // recorde the new dressed polygon in the vector obs
			    obs.addElement(dressed_obstacles[i]);
			}
		}
	    }
	}

	// create (dressed) gems
	for (i=0; i<num_gem; i++)
	{
	    D_Gem gem_tmp = new D_Gem();
	    success = false;
	    while (!success)
	    {
		success = true;
		x = Boundary.minx + gem_radius + 1 +
		    (int) Math.round((Boundary.maxx - Boundary.minx - 2*gem_radius -2) * Math.random());
		y = Boundary.miny + gem_radius + 1 + 
		    (int) Math.round((Boundary.maxy - Boundary.miny - 2*gem_radius -2) * Math.random());
		gem_tmp.dress(x, y, gem_radius, rover_width, rover_height);
		// see that the gem does not touch obstacles
		for (j=0; j<num_obstacle; j++)
		    success = success && (!gem_tmp.contains_poly(dressed_obstacles[j].body));
		// see that this gem does not touch previous gems
		for (j=i-1; j>=0; j--)
		    success = success && ( (gem_tmp.x-dressed_gems[j].x)*
				   (gem_tmp.x-dressed_gems[j].x)+ 
				   (gem_tmp.y-dressed_gems[j].y)*
				   (gem_tmp.y-dressed_gems[j].y) >
				   (gem_tmp.radius+dressed_gems[j].radius)*
				   (gem_tmp.radius+dressed_gems[j].radius) );
		if(success)
		    {
			dressed_gems[i].dress (x, y, gem_radius, rover_width, rover_height);
			gems.addElement(dressed_gems[i]);
		    }
	    }
	}

	// create rover
	success = false;
	while (!success)
	    {
		success = true;
		x = Boundary.minx + (int) Math.round((Boundary.maxx - Boundary.minx) * Math.random());
		y = Boundary.miny + (int) Math.round((Boundary.maxy - Boundary.miny) * Math.random());
		int tmp_angle = (int) Math.round(360 * Math.random());
		rover.put (x, y, rover_width, rover_height, tmp_angle);
		// see that the rover is contained in the boundary
		success = success && Boundary.contains(rover.body);
		// see that the rover does not initially hit obstacles
		success = success && (rover.hit_obstacle(dressed_obstacles, num_obstacle) == -1);
		// see that the rover does not initially hit gems
		success = success && (rover.hit_gem(dressed_gems, num_gem) == -1);
	    }
	repaint();
	animator = new Thread(this);
    }

public void start()
    {
       	// Event myevent = new Event(Restart,1004,"Restart");
	if (animator.isAlive())
	{
		animator.resume();
	}
	else
	{
	    // if animator is not allocated
	    // (not "new") it's wrong to start.
	    animator.start();
	}
    }
    
public void stop()
    {
	getAppletContext().showStatus("animator thread stoped");
	animator.suspend();
    }
public void destroy()
    {
	// nomenclature of Applet mismatches Thread. 
	animator.stop();
	backBufferG.dispose();
	backBuffer.flush();
    } 
    
public void run()
    {

	int i, j, k;
	int curr_gem;
	double dis_min, dis;
	boolean blocked , success , blocked_tmp;
	//Point p = new Point(10,10);
	//Point p_ori = new Point(0,0), p_tar = new Point(0,0);

	//calculate path using simulated annealling

	 try
	    {
		Thread.sleep (delay);
	    }
	    catch (InterruptedException e)
	    {

	    }

	   
	while(gems.size()!=0)
	{
	    
	    blocked = false;
	    success = false;
	    // p1 is the point where the rover is current at
	    p1.x = rover.cmx;
	    p1.y = rover.cmy;

	    
	    curr_gem=0;
	    dis_min = 10000000000; // the diagonal of input image
	    // find the closest gem to the rover
	    // record the locate of that rover by p2, and index curr_gem
	    for (i=0;i<gems.size();i++)
	    {
		p2.x = ((D_Gem)gems.elementAt(i)).x;
		p2.y = ((D_Gem)gems.elementAt(i)).y;
		if (dis_2(p1,p2) < dis_min)
		{
		    dis_min = dis_2(p1,p2);
		    curr_gem = i;
		}
	    }
	    System.out.println("curr_gem="+ curr_gem);

	    // p2 is point where the target gem is located
	    p2.x = ((D_Gem)gems.elementAt(curr_gem)).x;
	    p2.y = ((D_Gem)gems.elementAt(curr_gem)).y;	    

	    // if the rover hit the target gem, remove the gem
	    // from the gem vector list
	    if (rover.hit_gem((D_Gem)gems.elementAt(curr_gem)))
		{
		    gems.removeElementAt(curr_gem);
		    break;
		}
	    
	    success = smartGo(dressed_obstacles, num_obstacle,
				  gems, p1, p2);
	    /***********************
	    // check if there is obstacles bw the rover
	    // and the target gem
	    for(j=0;j< obs.size(); j++)
	    {
		Polygon poly = new Polygon();
		poly = ((D_Obstacle) obs.elementAt(j)).boundpoly;
		if(intersect(p1,p2,poly)){ blocked = true; break;}
	    }

	    // if there is no obstacles bw, go for the gem
	    if(!blocked)
		success = smartGo(dressed_obstacles, num_obstacle,
				  gems, p1, p2);

	    // if there is obstacles in between, record in vector Myscene all
	    // the vertices of the boundarypolygon of all
	    // obstacles that can be seen by the rover

	    else
	    {
		myScene = findScene(p1, obs, gems);
		System.out.println("here:"+myScene.size());	
		for(i=0;i<myScene.size();i++)
		    System.out.println( (Point)myScene.elementAt(i));
		animator.suspend();
		//myPath.addElement( p2 );
		//findPath(myScene, p2, obs, gems);
		//myPath.addElement( p1 );
		//System.out.println("here2");	
		//for(j=myPath.size()-1 ;j>0; j--)
		//{
		//   System.out.println("here3");	
		//    p_ori = (Point)myPath.elementAt(j);
 		//    p_tar = (Point)myPath.elementAt(j-1);
 		//    success = smartGo(dressed_obstacles, num_obstacle,
		//		      gems, p_ori, p_tar);
		//}
		//myPath.removeAllElements();
		//myScene.removeAllElements();
	    }
	    
	    System.out.println("here4");	
	    
	    else
	    {
		p_inter = 16;
		tmp_n = p_inter + 1;
		while(!success)
		{
		    inter_p.removeAllElements();
		    p1.x = rover.cmx;
		    p1.y = rover.cmy;
		    // using annealling
		    inter_p.addElement(p1);
		    
		    for (j=1; j< tmp_n; j++)
		    {
			p.x = p1.x + (p2.x-p1.x)/tmp_n*j;
			p.y = p1.y + (p2.y-p1.y)/tmp_n*j;
			inter_p.addElement(p);
		    }
		    
		    inter_p.addElement(p2);
		    anneal(obs, num_obstacle, p1, p2, p_inter);
		    boolean su_step = true;;
		    for(j=0; (j<tmp_n) && su_step; j++)
		    {
			p_ori = (Point)inter_p.elementAt(j);
			p_tar = (Point)inter_p.elementAt(j+1);
			su_step = smartGo(dressed_obstacles, (D_Gem)gems.firstElement(),
					  num_obstacle, p_ori, p_tar);
		    }
		   success =  su_step;

		}
	    }*********************************/

	    
	}
	
    }
    
public void update(Graphics g)
    {
	// update(), if not overridden, will
	// 1)  clear g  2) set colors 3) call paint().
	// The "clear g" part is the reason screen flashes.
	// We can avoid the problem using double buffering.
	paint(g);
    }
        
public void paint(Graphics g)
    {
	int i;
	Boundary.show(backBufferG);
	rover.show(backBufferG);
	for (i=0; i<num_gem; i++) dressed_gems[i].show(backBufferG);
	for (i=0; i<num_obstacle; i++) dressed_obstacles[i].show(backBufferG);
	backBufferG.setColor(Color.yellow);
	for(i=0; i< inter_p.size(); i++)
	{
	    draw_pointer(backBufferG, ((Point)inter_p.elementAt(i)).x, ((Point)inter_p.elementAt(i)).y, 2);
	}

	g.drawImage(backBuffer,0,0,this);
	    
    }

public void anneal(Vector obs, int obs_num, Point p1, Point p2, int p_inter)
    {
	Point p = new Point(0, 0);
	int i, j, in;
	int[] p_in = new int[p_inter + 2]; //index for if a point is in the obs
	double frac = 10000000000; // if the point is in a obstacle, add energy
	double Energy, de, E_norm;
	double step = 10; // max change of x and y in one step;
	double TFACTOR = 0.9; //annealing schedule, reduce t by this factor on each step.
	double t;

	System.out.println("in anneal");
	p_in[0] = 0;
	p_in[tmp_n] = 0;
	Energy = 0.0;
	t = 100;
	E_norm = dis_4(p1,p2)/(tmp_n*tmp_n*tmp_n);

	for (i=0;i<tmp_n;i++)
	{
	    p_in[i] = 0;
	    Energy += dis_4((Point)inter_p.elementAt(i+1), (Point)inter_p.elementAt(i));
	    // to check if the point is in the obstacles
	    for(j=0; j<obs_num; j++)
	    {
		if (((Polygon)obs.elementAt(j)).contains((Point)inter_p.elementAt(i)))
			{
			    p_in[i] = 1;
			    break;
			}
	    }
	    Energy += frac*p_in[i];
	}
	Energy /= E_norm;

	for (j=1; t>0.000001; j++) 
	{
	    for (int m=0; m<10; m++)
	    {
		for(i=1;i<tmp_n; i++)
		{
		    Point new_p = new Point();
		    new_p.x =(int) (((Point)inter_p.elementAt(i)).x + (Math.random() - 0.5)*step);
		    new_p.y =(int) (((Point)inter_p.elementAt(i)).y + (Math.random() - 0.5)*step);
		    de = dis_4((Point)inter_p.elementAt(i+1), new_p) +
			dis_4((Point)inter_p.elementAt(i-1), new_p) -
			dis_4((Point)inter_p.elementAt(i+1), (Point)inter_p.elementAt(i)) -
			dis_4((Point)inter_p.elementAt(i-1), (Point)inter_p.elementAt(i));
		    in = 0;
		    for (int k=0; k<obs_num; k++)
		    {
			if (((Polygon)obs.elementAt(k)).contains(new_p))
			    in = 1;
		    }
		    de = de - frac*p_in[i] + frac*in;
		    de /= E_norm;
		    if (metrop(de,t))
		    {
			inter_p.setElementAt(new_p,i);
			p_in[i] = in;
			Energy += de;
		    }
		}
	    }
	    t *= TFACTOR;
	}
    }

static public double dis_1(Point p1, Point p2)
    {
	return( Math.abs(p1.x - p2.x)+Math.abs(p1.y - p2.y) );
    }

static public double dis_2(Point p1, Point p2)
    {
	return( (((double)p1.x - (double)p2.x)*((double)p1.x -(double) p2.x)+
		 ((double)p1.y - (double)p2.y)*((double)p1.y -(double) p2.y))*
		(((double)p1.x - (double)p2.x)*((double)p1.x -(double) p2.x)+
		 ((double)p1.y - (double)p2.y)*((double)p1.y -(double) p2.y))
		);
    }    
static public double dis_4(Point p1, Point p2)
    {
	if (dis_1(p1, p2) < 10) return 1000.0;
	return(   ((double)p1.x - (double)p2.x) * ((double)p1.x - (double)p2.x) *
		  ((double)p1.x - (double)p2.x) * ((double)p1.x - (double)p2.x) +
		  ((double)p1.y - (double)p2.y) * ((double)p1.y - (double)p2.y) *
		  ((double)p1.y - (double)p2.y) * ((double)p1.y - (double)p2.y)) ;
    }
    
static public boolean metrop(double de, double t)
    {
        return ((de < 0.0) || (Math.random()<Math.exp(-de/t)));
    }

public boolean smartGo( D_Obstacle obstacles[], int num_obstacle ,
			Vector gems, Point p1, Point p2)
    {
	//p2 is the destination
	int ddx, ddy, ddx_abs, ddy_abs, dx, dy, move_i;
	int theta=0;
	boolean success = false;
	Vector passed_point = new Vector();
	Point p ;
	Object p_curr = (Object) new Point();
	
	while(true)
	{
	    ((Point)p_curr).x = rover.cmx;
	    ((Point)p_curr).y = rover.cmy;
	    
	    if ( ((Point)p_curr)==p2 ) return true;
	    
	    ddx = p2.x - ((Point)p_curr).x;
	    ddy = p2.y - ((Point)p_curr).y;
	    ddx_abs = Math.abs(ddx);
	    ddy_abs = Math.abs(ddy);
	    if (ddx_abs==0) dx = 0;
	    else dx = ddx/ddx_abs;
	    if (ddy_abs==0) dy = 0;
	    else dy = ddy/ddy_abs;
	    
	    
	    System.out.println("in smartGo");
	    //while((ddx_abs!=0)||(ddy_abs!=0))
	    //{
	    try
	    {
		Thread.sleep (delay);
	    }
	    catch (InterruptedException e)
	    {
		
	    }
	    if ( rover.hit_gem(gems)!= (-1))
	    {
		gems.removeElementAt(rover.hit_gem(gems));
		return true;
	    }
	    //if (ddx_abs<=0) dx = 0;
	    //if (ddy_abs<=0) dy = 0;
	    
	    
	    if( (!passed_point.contains( p= new Point(((Point)p_curr).x+dx, ((Point)p_curr).y+dy))) &&
		(dx!=0) && (dy!=0) &&
		rover.move(dx,dy,obstacles,num_obstacle))
	    {
		
		passed_point.addElement(((Point)p_curr));
	    }
	    else if( (!passed_point.contains( p= new Point(((Point)p_curr).x+dx, ((Point)p_curr).y))) &&
		     (dx != 0) 
		     && rover.move(dx,0,obstacles,num_obstacle))
	    {
		passed_point.addElement(((Point)p_curr));
	    }
	    else if( (!passed_point.contains( p= new Point(((Point)p_curr).x, ((Point)p_curr).y+dy)))
		     && (dy != 0) &&
		     rover.move(0,dy,obstacles,num_obstacle))
	    {
		passed_point.addElement(((Point)p_curr));
	    }
	    else if( (!passed_point.contains( p= new Point(((Point)p_curr).x+dx, ((Point)p_curr).y-dy)))
		     && rover.move(dx,-dy,obstacles,num_obstacle))
	    {
		passed_point.addElement(((Point)p_curr));
	    }
	    else if( (!passed_point.contains( p= new Point(((Point)p_curr).x-dx, ((Point)p_curr).y+dy)))
		     && rover.move(-dx,dy,obstacles,num_obstacle))
	    {
		passed_point.addElement(((Point)p_curr));

	    }
	    else if( (!passed_point.contains( p= new Point(((Point)p_curr).x-dx, ((Point)p_curr).y)))
		     && rover.move(-dx,0,obstacles,num_obstacle))
	    {
		passed_point.addElement(((Point)p_curr));
	    }
	    else if( (!passed_point.contains( p= new Point(((Point)p_curr).x, ((Point)p_curr).y-dy)))
		     && rover.move(0,-dy,obstacles,num_obstacle))
	    {
		passed_point.addElement(((Point)p_curr));

	    }
	    else if( (!passed_point.contains( p= new Point(((Point)p_curr).x-dx, ((Point)p_curr).y-dy)))
		     && rover.move(-dx,-dy,obstacles,num_obstacle))
	    {
		passed_point.addElement(((Point)p_curr));

	    }
	    
	    /*
	      else 
	      {
	    if(first_time)
	    {
	    if (rover.rotate(5, obstacles, num_obstacle)!=0)
	    {theta = 5; first_time = false; } 
	    else if (rover.rotate(-5, obstacles, num_obstacle) !=0)
	    {theta = -5; first_time = false;}
	    else return false;
	    System.out.println("rotate "+theta+" first_time = "+ first_time);
	    }
	    else
	    {
	    System.out.println("loop break 5");
	    if (rover.rotate(theta, obstacles, num_obstacle)!=0){}
	    else return false;
	    }*/
	}
	    
    }
    
static public void draw_pointer(Graphics g, int x, int y, int size)
    {
	int px[] = {x+size, x,      x-size, x  };
	int py[] = {y,      y+size, y,      y-size};
	g.fillPolygon (px,py,4);
    }
    
static public boolean intersect(Point p1, Point p2, Polygon p)
    {
	int i;
	Point p_poly1 = new Point(p.xpoints[p.npoints-1], p.ypoints[p.npoints-1]),
	    p_poly2 = new Point(p.xpoints[0], p.ypoints[0]);
	if ((p1==p_poly1) || (p1==p_poly2) || (p2==p_poly1) || (p2==p_poly2))
	    return false;
	if ( intersection(p1, p2, p_poly1, p_poly2) )
		return true;
	for(i=0; i< p.npoints-1; i++)
	{
	    p_poly1.x = p.xpoints[i];
	    p_poly1.y = p.ypoints[i];
	    p_poly2.x = p.xpoints[i+1];
	    p_poly2.y = p.ypoints[i+1];
	    if ((p1==p_poly1) || (p1==p_poly2) ||
		(p2==p_poly1) || (p2==p_poly2))
	    return false;
	    if ( intersection(p1, p2, p_poly1, p_poly2) )
		return true;
	}

	return false;
    }
	
static public boolean intersection(Point p1a,Point p2a, Point p1b,Point p2b)
{
    return ( ( ccw(p1a,p2a,p1b) * ccw(p1a,p2a,p2b)) <=0)
	&& (( ccw(p1b,p2b,p1a) * ccw(p1b,p2b,p2a)) <= 0);
}

static public int ccw(Point p1, Point p2, Point p3)
	{
	    double dx1, dx2, dy1, dy2;
	    dx1 = p2.x  - p1.x ;
	    dx2 = p3.x  - p1.x ;	    
	    dy1 = p2.y  - p1.y ;
	    dy2 = p3.y  - p1.y ;
	    if ( dx1*dy2 > dy1*dx2) return +1;
	    if ( dx1*dy2 < dy1*dx2) return -1;
	    if ((dx1*dx2<0) || (dy1*dy2<0)) return +1;
	    if ((dx1*dx1+dy1*dy1) < (dx2*dx2+dy2*dy2)) return +1;
	    return +1;
	}
    /*    
public Vector findScene(Point p1, Vector obs, Vector gem)
    {
	// ps is the viewer, myScene is a vector difine in the
	// main function who call this method to record all the
	// visible vertices and gems

	int l, j, k, index_scene=-1;
	boolean blocked = false;
	Point p_test = new Point();
	Vector scene = new Vector();
	for(j=0; j< gem.size(); j++)
	{
	    p_test.x = ((D_Gem)gem.elementAt(j)).x;
 	    p_test.y = ((D_Gem)gem.elementAt(j)).y;	    
	    blocked = false;
	    for (k=0; k< obs.size(); k++)
	    {
		if(intersect(p1, p_test, ((D_Obstacle)obs.elementAt(k)).boundpoly))
		{ blocked = true; break;}
	    }
	    if(!blocked)
		{
		    System.out.println(p_test);
		    scene.addElement(p_test);
		    index_scene++;
		    System.out.println(scene.elementAt(index_scene));
		    
		}
	}
	
	for(j=0; j< obs.size(); j++)
	{
	    for(l=0; l<((D_Obstacle)obs.elementAt(j)).boundpoly.npoints ; l++)
	    {
		p_test.x = ((D_Obstacle)obs.elementAt(j)).boundpoly.xpoints[l];
		p_test.y = ((D_Obstacle)obs.elementAt(j)).boundpoly.ypoints[l];
		if( (p_test.x==p1.x) && (p_test.y==p1.y)) break;
		blocked = false;
		for (k=0; k< obs.size(); k++)
		{
		    if(intersect(p1, p_test, ((D_Obstacle)obs.elementAt(k)).boundpoly))
		    { blocked = true; break;}
		}
		if(!blocked)
		    {
			System.out.println(p_test);
			scene.addElement(p_test);
			index_scene++;
			System.out.println(scene.elementAt(index_scene));
		    }
	    }
	}
	System.out.println("scene.size()=(in find)" +scene.firstElement() +scene.lastElement() );
	for(j=0;j<scene.size();j++)
	{
		    System.out.println( (Point)scene.elementAt(j));
		  scene.setElementAt();  
	}
	System.out.println("secene from findScene"+scene.size());
	return scene;
    }

public void findPath(Vector scene, Point target, Vector obs, Vector gems)
    {
	//return a point in the path, myScene is a local parameter
	//myPath is a global parameter to store the points in the path.
	//this method is called recursively to find a path
	int i, j ;
	Vector myScene = new Vector();	
	Point p_parent = new Point(), p_son = new Point();
	
	System.out.println("in findPath");
	
	for (i=0; i< scene.size(); i++)
	{
	    System.out.println("sence.size="+scene.size()+ " i="+i);
	    p_parent = (Point)scene.elementAt(i);
	    findScene( p_parent, obs, gems);
	    System.out.println("myScene="+ myScene);
	    for (j=0; j< myScene.size(); j++)
	    {
		System.out.println("mySence.size="+myScene.size()+ "for j loop"+j);
		p_son = (Point)myScene.elementAt(j);
		if ( p_son == target) {myPath.addElement(p_parent); return;}
		else
		{
		    findPath(myScene, target, obs, gems);
		    return;
		}
	    }
	    
	}
    }
	
    */	
	
} // class Path ends