package Algorithms;

import edu.carleton.motion.*;
import java.io.*;
import java.util.*;
import java.awt.*;
import java.awt.geom.*;

/** Implementation of the line-of-sight motion planning algorithm. This 
*	algorithm solves a motion planning problem in the following fashion:
*	for each vertex of each polygonal obstacle, a line is drawn to each 
*	other vertex that it may "see." Seeing, in this sense, means any 
*	vertex from which we can draw a line to the current vertex such that
*	this line intersects no other obstacle. Once all such lines have been
*	drawn (or at least calculated), we perform the same operation for the
*	start and the goal points of the Worldspace. Once this process is
*	complete, there must exist an uninterrupted path from start to goal
*	along these lines.
*
*	@author Dave Flynn, Tim Whittemore, Jeremy Carr
*	@version 2/07/04
*/
public class LineOfSight implements MPAlgorithm
{
   private double[][] validLines;
   private Point[] reference;
    
    /** The motion planning method. Returns a vector of SolutionPath
     *	objects, each of which corresponds to one distinct Robot
     *	passed within the Scenario.
     */
    public Vector solve(Scenario problem)
    {
	//build the vector of solution paths that we will pass back to
	//UserInterface.java. Initialize the vector to have one slot
	//for each robot in the Scenario.
	Vector paths = new Vector(problem.getNRobots());
	
	//we set this to be twice the number of robots in the problem plus
	//the number of vertioes in the worldspace to account for the
	//robots' start and end points.
	int arraySize = GetTotalVertices(problem.getWorldspace()) + 
									 2 * problem.getNRobots();
	validLines = new double[arraySize][arraySize];
	InitMatrix(arraySize, problem.getWorldspace());
	reference = new Point[arraySize];
	
	ReadPoints(problem.getWorldspace(), problem);

	BuildEdgeMatrix(problem.getWorldspace());
	
	int startIndex = 0;
	int endIndex = 0;
	
	//perform these tasks for each robot, once the edge matrix has been 
	//built. For each robot, find the index of its start and end point
	//in the reference array. Pass these indices to the shortestPath
	//algorithm, and record the shortest path for this robot--adding
	//it to the vector of solutionPath objects.
	for(int i = 0; i < problem.getNRobots(); i++)
	{
		//find the start and end indices in our array.
		SolutionPath sp = problem.getPath(i);
		for(int j = 0; j < arraySize; j++)
		{
			if(reference[j].equals(sp.getStart()))
			{
				startIndex = j;
			}
			if(reference[j].equals(sp.getEnd()))
			{
				endIndex = j;
			}
		}
	
		//find the shortest path through our edge matrix
		Vector v = shortest(arraySize, startIndex, endIndex);
	
		//build the solutionpath that we want to add to the vector of paths
		SolutionPath sp2 = new SolutionPath(reference[startIndex]);
		for(int k = 1; k < v.size(); k++)
		{	
		    sp2.addPoint(reference[((Integer)v.elementAt(k)).intValue()]);
		}
	
		paths.add(i, sp2);
	}
		
	return paths;
    }
	
    /** The workhorse of the Line of Sight algorithm. Given a Scenario,
     *	in particular the Worldspace, build a list of edges from one vertex
     *	to another for each vertex of each obstacle. For vertices that we
     *	have already connected, ignore the reverse edge to save time.
     */
    private void BuildEdgeMatrix(Worldspace w)
    {
	int len = reference.length;
	double distance = 0;
	Point v1, v2;
	
	for(int i = 0; i < len; i++)
	{
	    for(int j = i + 1; j < len; j++)
	    {
			v1 = reference[i];
			v2 = reference[j];
			
			if(!intersect(v1, v2, w))
		    {
			int diffX = (int)(reference[i].getX() - 
					  		  reference[j].getX());
			int diffY = (int)(reference[i].getY() - 
					  		  reference[j].getY());
			distance = Math.sqrt(diffX * diffX + diffY * diffY);
				
			validLines[i][j] = distance;
			validLines[j][i] = distance;
		    }
	    }
	}
	
    }

    /** Line intersection algorithm. For each pair of vertices in the 
     *	Worldspace, we must attempt to draw a line between them and check
     *	to see whether or not this line is legal; that is, if it intersects
     *	any of the obstacles in the Worldspace.
     */ 
    public boolean intersect(Point a, Point b, Worldspace w)
    {
	if(a.equals(b))
	    return false;
	
	Point half = new Point((int) (.5 * b.getX() + .5 * a.getX()), (int) (.5 * b.getY() + .5 * a.getY()));

	for(int j = 0; j < w.numObstacles; j++)
	{
	    Obstacle ob = w.getObstacle(j);
	    Polygon temp = (Polygon) ob.getPoly();
	    if (temp.contains(half))
		return true;
	    for(int i = 0; i < ob.getNumVertices(); i++)
	    {
		Point vertOne = ob.getPoint(i);
		Point vertTwo = ob.getPoint((i + 1) % ob.getNumVertices());
		
		//special case: if any vertex in the worldspace lies directly on this line,
		//return true. Our general line intersection algorithm does not catch this.
		if(isOn(a, b, vertOne))
		{
			return true;
		}
					
		if (straddles(a,b,vertOne,vertTwo) && straddles(vertOne, vertTwo, a, b))
		    return true;
	    }
	}
	return false;
    }

	private boolean isOn (Point A, Point B, Point C) 
	{
	double TOLERANCE = .001;
	
	Point Left = new Point();
	Point Right = new Point();
	Point Middle = new Point();
	//Put the points in order- catagorize A, B, and C as
	//Left, Right, and Middle
	if(A.equals(C) || B.equals(C))
		return false;
	
	
	if (A.getX() < B.getX()) {
	    if (C.getX() < A.getX()) {
		Left = C; Middle = A; Right = B;
	    } 
	    else if (C.getX() < B.getX()){
		Left = A; Middle = C; Right = B;
	    } else {
		Left = A; Middle = B; Right = C;
	    }
	} else { //B < A
	    if (C.getX() < B.getX()) {
		Left = C; Middle = B; Right = A;
	    } 
	    else if (C.getX() < A.getX()){
		Left = B; Middle = C; Right = A;
	    } else {
		Left = B; Middle = A; Right = C;
	    }
	}
	
		//Infinite Slope case
	if (Left.getX() == Middle.getX() && Middle.getX() == Right.getX())
		if ((Left.getY() > Middle.getY() && Right.getY() < Middle.getY()) ||
			(Left.getY() < Middle.getY() && Right.getY() > Middle.getY()))
			return true;
			
	
	
	double slope = (double)(Left.getY() - Right.getY())/
			(Left.getX() - Right.getX());
	double projectedY = (double) (Left.getY() + 
				      slope*(Middle.getX() - Left.getX()));
		
	if (Math.abs(projectedY - Middle.getY()) < TOLERANCE)
	    return true;
	return false;
    }
	
    /** Determines the total number of vertices in the worldspace. This means
     *	that, for each obstacle present in the worldspace, we find out how
     *	many vertices it has, and add them all up. This is neccesary to
     *	create the 2D array that we will use to store the lines.
     */
    private int GetTotalVertices(Worldspace w)
    {
	int vertexCount = 0;
	Obstacle obs;
	
	//for each obstacle in the worldspace, get the number of vertices
	//it has, and add them to a counter variable we keep track of.
	for(int i = 0; i < w.numObstacles; i++)
	{
	    obs = w.getObstacle(i);
	    vertexCount += obs.getNumVertices();
	}
		
	return vertexCount;
    }
	
    /** Initialize the matrix that we need. This is a 2-dimensional
     *	array used to store relationships between vertices, where the
     *	relationship stored is the distance between two vertices. If a line
     *	from one vertex to the other cannot be drawn without intersecting
     *	an obstacle, then we set the value of the matrix[x,y] to be -1,
     *	where x and y are the two vertices in question. Similarly, the matrix
     *	element [y,x] is set to -1, since the two elements reference the same
     *	line. If the line can be legally drawn, the matrix element is a double
     *	indicating the distance from one point to the other. Finally, if
     *	the two points are the same, the value of the element [x,x] is 
     *	defined to be 0. 
     */
    private void InitMatrix(int size, Worldspace w)
    {
		for(int i = 0; i < size; i++)
		{
	    	for(int j = 0; j < size; j++)
	    	{	
				//if we're looking at the intersection in the matrix of
				//the same point, set this slot to zero. Otherwise,
				//set the elements [x,y] and [y,x] to -1 for now.
				if(i == j)
		   			validLines[i][j] = 0;
				else
				{
		    		validLines[i][j] = -1;
		    		validLines[j][i] = -1;
				}
	    	}
		}			 
    }
	
    /** Read each point of each obstacle in the worldspace in turn. Copy
     *	this point into the reference array. This array is used to reference
     *	the vertices of the matrix "validLines," which stores the distance
     *	between each vertex. The index of the reference array corresponds
     *	to the index of the matrix; the point at index i of the reference
     *	array stores its relationship to other points in the Worldspace
     *	in the row [i, x] of the matrix and the column [x,i] of the matrix,
     *	where x represents the index of any other point in the reference
     *	array. The reference array has already been set to the proper size.
     */
    private void ReadPoints(Worldspace w, Scenario problem)
    {
		int pointCnt = reference.length;
		int curPoint = 0;
		int startObsPoint = 0;
		Obstacle obs;
		Point p = new Point();
		SolutionPath sp = new SolutionPath();
		Point v1, v2;
		
		//for each obstacle in w...
		for(int i = 0; i < w.numObstacles; i++)
		{
	    	obs = w.getObstacle(i);
			
			//for wraparound, we have to keep track of where in the matrix
			//the first (0 index) point of a particular obstacle is stored.
			startObsPoint = curPoint;
			
			//for each vertex of obs...
	    	for(int j = 0; j < obs.getNumVertices(); j++)
	    	{
				p = obs.getPoint(j);
				reference[curPoint] = new Point(p);
				
				//take care of the special case of edge detection within a 
				//single obstacle. Adjacent vertices of an obstacle form valid
				//edges for the purposes of our edge matrix, as we would like 
				//the algorithm to follow the edge of an obstacle if need be.
				v1 = obs.getPoint(j);
				
				//take care of wraparound, for last point of obs compared to first
				if((j+1) >= obs.getNumVertices())
					v2 = obs.getPoint(0);
				else
					v2 = obs.getPoint(j+1);
				int diffX = (int)(v1.getX() - v2.getX());
				int diffY = (int)(v1.getY() - v2.getY());
				double distance = Math.sqrt(diffX * diffX + diffY * diffY);
				
				//second part of wraparound special case
				if((j+1) >= obs.getNumVertices())
				{
					validLines[curPoint][startObsPoint] = distance;
					validLines[startObsPoint][curPoint] = distance;
				}
				else
				{
					validLines[curPoint][curPoint + 1] = distance;
					validLines[curPoint+1][curPoint] = distance;
				}
				curPoint++;	
	    	}
		}
	
		for(int i = 0; i < problem.getNRobots(); i++)
		{
	    	sp = problem.getPath(i);
	    	reference[curPoint] = new Point(sp.getStart());
	    	curPoint++;
	    	reference[curPoint] = new Point(sp.getEnd());
	    	curPoint++;
		}
    }

    /** Reads in 4 points, returns true if the line between point c and d 
     *  intersects the line between a and b.
     */
    
    public boolean straddles(Point a, Point b, Point c, Point d)
    {
       	Point v1 = new Point((int)(b.getX() - a.getX()),
			     (int)(b.getY() - a.getY()));
		Point v2 = new Point((int)(c.getX() - a.getX()),
			     (int)(c.getY() - a.getY()));
		Point v3 = new Point((int)(d.getX() - a.getX()),
			     (int)(d.getY() - a.getY()));
	if (((v1.getX() * v2.getY()) - (v1.getY() * v2.getX())) * 
	    ((v1.getX() * v3.getY()) - (v1.getY() * v3.getX())) < 0)
	    return true;
	else 
	    return false;
    }
    
    /** Takes a scenario and returns a vector of solution paths.
     */
    public Vector computePaths(int arraySize, Point[] reference, double[][] edges, Scenario problem)
    {
	System.out.println("in computePaths");
	Vector pathsToReturn = new Vector();
	for (int i = 0; i < problem.getNRobots(); i++)
	{
	    Point start = (Point) ((SolutionPath) ((Vector) problem.getPathList()).get(i)).getStart();
	    Point end = (Point) ((SolutionPath) ((Vector) problem.getPathList()).get(i)).getEnd();
	    int startIndex = 0;
	    int endIndex = 0;
	    for (int j = 0; j < arraySize; j++)
	    {
		Point temp = (Point) reference[j];
		if (temp.equals(start))
		    startIndex = j;
		else if (temp.equals(end))
		    endIndex = j;
	    }
	    pathsToReturn.add((SolutionPath) onePath(arraySize, edges, startIndex, endIndex, reference));
	}
	return pathsToReturn;
    }
    
    /** Returns a solutionpath for a single set of start and end points.
     */
    public SolutionPath onePath(int arraySize, double[][] edges, int startIndex, int endIndex, Point[] reference)
    {
	System.out.println("In OnePath");
	SolutionPath computedPath = new SolutionPath();
	computedPath.addPoint((Point) reference[startIndex]);
	computedPath.addPoint((Point) reference[startIndex]);
	computedPath.addOrientation(0);
	computedPath.addOrientation(0);
	
	int[] f = new int[2 * arraySize];
	int lf = 0;	//length of f
	double[] path = new double[arraySize];	//length of shortest path from startIndex to i
	int[] touch = new int[arraySize];
	double[] length = new double[arraySize];

	//initialize stuff...
	path[startIndex]=0;
	System.out.println("Entering For Loop 1");
	for (int i = 0; i < arraySize; i++)
	{
	    System.out.println("In two");
	    touch[i]=startIndex;  
	    length[i] = edges[startIndex][i];
	}	//for i
	System.out.println("Done For Loop 1");

	boolean keepLooking = true;
	System.out.println("Entering While Loop 1");
	for (int count = 0; count < arraySize && keepLooking; count++)
	{ 
	    System.out.println("In three");
	    double  min = 100000000;
	    int vnear = startIndex;
	    for (int i = 0; i < arraySize; i++)
		if (length[i]>0.0 && length[i]<min)
		{ 
		    min=length[i]; 
		    vnear=i;
		    System.out.println(vnear);
		} 

	    if (vnear == endIndex)
		keepLooking = false;
	    
	    //add new edge to F
	    f[lf]=touch[vnear]; f[lf+1]=vnear;
	    lf+=2;

	    //compute length of path from 1 to vnear
	    path[vnear]= (double) ((double) path[touch[vnear]]+ (double) edges[touch[vnear]][vnear]);
	    //update length and touch
	    for (int i = 0; i < arraySize; i++)
	    {
		if (edges[vnear][i]>0 && length[vnear]+edges[vnear][i]<length[i])
		{
		    length[i]=length[vnear]+edges[vnear][i];
		    touch[i]=vnear;
		}
	    }
	    System.out.println(keepLooking);
	    length[vnear]=-1;
	}
	
	System.out.println("BOO");
	System.out.println(path);
	return computedPath;
	
    }
	
	/** Implements Floyd's algorithm, finding the shortest path from
	 *  any point to any other point in the worldspace. We write a
	 *  custom algorithm to get the shortestpath out of the history matrix.
	 *  Once we have it, create a SolutionPath object with
	 *  the appropriate members, and return it to the interface.
	 */
	private Vector shortest(int n, int start, int target) {
	// shortest takes an nxn matrix validLines of edge validLiness and produces
	// an nxn matrix shortestPath of lengths of shortest paths, 
	//and an nxn matrix P giving the node history to get to that node
	int i,j,k;
	int [][] history = new int [n][n];
	double [][] shortestPath = new double[n][n];

	//init the three matrices
      for (i=0; i<n; i++)
	  for (j=0; j<n; j++) {
		  shortestPath[i][j] = validLines[i][j];
		  if (shortestPath[i][j] == -1) shortestPath[i][j] = Integer.MAX_VALUE;
		  history[i][j] = -1;
        }
      for (i=0; i<n; i++)
	  shortestPath[i][i] = 0;              // no self cycle
      
      //Grunt work- Is edge ik+kj shorter than ij?  If so, update the solution matrices
      for (k=0; k<n; k++)
	  for (i=0; i<n; i++)
	      for (j=0; j<n; j++) {
		  
		  if (shortestPath[i][k] > 0 &&
		      shortestPath[k][j] > 0 &&
		      shortestPath[i][j] > 0 &&
		      shortestPath[i][k]+shortestPath[k][j] < shortestPath[i][j]) {
		      //two edges are shorter than one, update the shortestPath matrix
		      shortestPath[i][j] = shortestPath[i][k] + shortestPath[k][j];
		      shortestPath[j][i] = shortestPath[i][k] + shortestPath[k][j];
		      history[i][j] = k;        // k is included in the shortest path
		      history[j][i] = k;
		  }
	      }

      //Traverse history correctly - get from 0 to target
      Vector path = new Vector();
      traverseHistory(start, target, history, path);
      path.insertElementAt(new Integer(start),0);
      path.insertElementAt(new Integer(start),0);
      path.addElement(new Integer(target));
      
      return path;
	      
    } //shortest
	
    /** Returns an answer vector that correctly traverses the history matrix returned
     *  by floyd's algorithm.
     */
    public void traverseHistory(int start, int end, int history[][], Vector answer) {

	if (history[start][end] == -1) {    
	    return;
	}
	traverseHistory(start, history[start][end],history, answer);
	answer.addElement(new Integer(history[start][end]));
	traverseHistory(history[start][end], end, history, answer);

    }

    /** Draw algorithm
     */
    public void draw( Graphics g ) 
    {
	int len = reference.length;
	Point start, end;
	g.setColor(Color.green);
	
	for(int i = 0; i < len; i++)
	{
	    for(int j = i+1; j < len; j++)
	    {
		start = reference[i];
		end = reference[j];
		if (validLines[i][j] > -1)
		    g.drawLine((int)start.getX(), (int)start.getY(),
			       (int)end.getX(), (int)end.getY());
	    }
	}

	
    }
}