package Algorithms;

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

/**     Implementation of the Voronoi Diagram Algorithm.  Right now it does
 *      nothing but draw the Voronoi Diagram.
 *
 *
 *      Things to do:
 * Make each edge a bunch of pts to make polygons 1 object.
 * Make edges of the worldspace hard by adding pts to them.
 * Get Dijkstra's algorithm from the other group to plug
 *     the point matrix into.
 * Figure out how to get calculations out of draw and into solve.
 *
 *
 *
 *
 *	@author Robert Hirai
 *	@version 2/2/04
 */
public class Voronoi implements MPAlgorithm
{
    Worldspace w;
    int obsCount;
    Vector pointPath;
    Vector ipts;
    Vector tempPts;
    SolutionPath tempPath;
    Point start, end;
    Point2D.Double tempPt, checkPt;
    int nextPt;
    int ptCount;
    Vector paths;
    Vector yellow = new Vector();
    Vector whiteStart = new Vector();
    Vector whiteEnd = new Vector();
    private Point[] p = new Point[100000];
    private Vector pstar = new Vector();
    private Color col1,col2,col3;
    private int N,taka,haba;
    private int k,i,j,l,n;
    private double di2,di,cp2,cpx,ys,t;
    private double x0,y0,xx,yy,xa1=0,ya1=0,yy2;
    private double di4,di3,cp3,cpx3,ys3,t2,ds,us;
    private double y20,y21,sa0,sa1,sa2,sa3,sa4,sa5;
    private int sa6,br,br2,u,k2;
    private int xz,xz2,yz,yz2;
    private String NS,habaS,takaS;
    private double Nd,takad,habad;
    private double kx[]=new double[100000];
    private double ky[]=new double[100000];
    private double kz[]=new double[100000];
    private double x1[]=new double[100000];
    private double y1[]=new double[100000];
    private int x[]=new int[100000];
    private int y[]=new int[100000];
    private double s[]=new double[100000];
    private String sss[]=new String[100000];
    
    /**     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)
    {
	System.out.println("solve()"); 


	
	paths = new Vector(problem.getNRobots());
	for(int i = 0; i < problem.getNRobots(); i++)
	    {
		paths.add(i, new SolutionPath());
	    }

	for (int i=0; i < problem.getNRobots(); i++) {
	    pointPath = new Vector(2);
	    tempPath = (SolutionPath) problem.getPath(i);
	    start = tempPath.getStart();
	    System.out.println("0) Adding point " + start);
	    pointPath.add(0, start); // Add the start point to the path.
	    // p[0]=start;
	    pstar.add(start);
	    System.out.println("adding start to p");
	    //ptCount++;
	    end = tempPath.getEnd();

	    System.out.println("1) Adding point " + end); //
	    pointPath.add(end);
	    pstar.add(end);
	}
	//System.out.println("1) Adding point " + end); //
	//pointPath.add(end);
	
	System.out.println("pointPath=" + pointPath); //
	((SolutionPath)paths.get(i)).setPath(pointPath);

	init(problem);

	w = problem.getWorldspace();
	obsCount = w.getNumObstacles();


	Point old,current;
	N=n;
	System.out.println("n="+n);
	for(k=0;k<N;k++){ //Calculate Yellow Points
	    x1[k]=p[k].x+Math.cos(k*1.1)+k*0.0001;
	    y1[k]=p[k].y+Math.sin(k*1.1)+k*0.001;
	    x[k]=(int)(x1[k]);
	    y[k]=(int)(y1[k]);
	    s[k]=jou(x1[k]*x1[k]+y1[k]*y1[k],0.5);
	    //    System.out.println("Drawing point ("+(x[k]-2)+","+(y[k]-2)+")");
	    yellow.add(new Point(x[k]-2,y[k]-2));
	    //System.out.println("Adding to yellow");
	    //g.fillOval(x[k]-2,y[k]-2,4,4);  // Looks like this is a point.
	}
	if(N>1){
	    //g.setColor(col3);
	    for(i=1;i<=N-1;i++){
		for(j=i+1;j<=N;j++){
		    br=0;
		    di2=(y1[i-1]-y1[j-1])/(x1[i-1]-x1[j-1]);
		    di=-1/di2;
		    cp2=(y1[i-1]+y1[j-1])/2;
		    cpx=(x1[i-1]+x1[j-1])/2;
		    ys=cp2-cpx*di;
		    t=jou(x1[i-1]-x1[j-1],2)+jou(y1[i-1]-y1[j-1],2);
		    if(ys>0 && ys<taka){
			x0=0;y0=ys;
		    }
		    else{
			if(di>0){
			    x0=-ys/di;y0=0;
			}
			else{
			    x0=(taka-ys)/di;
			    y0=taka;
			}
		    }
		    yy=di*haba+ys;
		    if(yy>0 && yy<taka){
			xa1=haba;ya1=yy;
		    }
		    else{
			if(di>0){
			    xa1=(taka-ys)/di;ya1=taka;
			}
			else{
			    xa1=-ys/di;
			    ya1=0;
			}
		    }
		    l=1;
		    kx[l-1]=x0;ky[l-1]=y0;
		    sa2=x1[j-1]-x1[i-1];
		    sa4=y1[j-1]-y1[i-1];
		    for(k=1;k<=N;k++){
			if(k!=i && k!=j){
			    di4=(y1[i-1]-y1[k-1])/(x1[i-1]-x1[k-1]);
			    di3=-1/di4;
			    cp3=(y1[i-1]+y1[k-1])/2;
			    cpx3=(x1[i-1]+x1[k-1])/2;
			    ys3=cp3-cpx3*di3;
			    t2=jou(x1[i-1]-x1[k-1],2)+jou(y1[i-1]-y1[k-1],2);
			    y20=di3*x0+ys3;
			    y21=di3*xa1+ys3;
			    sa0=y0-y20;
			    sa1=ya1-y21;
			    sa3=x1[k-1]-x1[i-1];
			    sa5=y1[k-1]-y1[i-1];
			    if(sa2*sa3>0 && sa4*sa5>0){
				sa6=1;
			    }
			    else{
				sa6=0;
			    }
			    if(sa0*sa1>0 && t>t2 && sa6==1){
				br=1;
				break;
			    }
			    if(sa0*sa1<0 || t<t2 || sa6!=0){
				if(sa0*sa1<0 || t>t2){
				    l++;
				    kx[l-1]=(ys3-ys)/(di-di3);
				    ky[l-1]=di*kx[l-1]+ys;
				}
			    }
			}//if(k!=i && k!=j)
		    }//next k
		    if(br==0){
			l++;
			kx[l-1]=xa1;
			ky[l-1]=ya1;
			for(u=1;u<=l;u++){
			    kz[u-1]=0;
			}
			heapv(kx,ky,kz,l);
			for(k=1;k<=l-1;k++){
			    k2=k+1;
			    xx=(kx[k-1]+kx[k2-1])/2;
			    yy2=di*xx+ys;
			    ds=jou(xx-x1[i-1],2)+jou(yy2-y1[i-1],2);
			    br2=0;
			    for(u=1;u<=N;u++){
				if(u!=i && u!=j){
				    us=jou(xx-x1[u-1],2)+jou(yy2-y1[u-1],2);
				    if(us<ds){
					br2=1;
					break;
				    }
				}
			    }//next u
			    if(br2==0){
				xz=(int)(kx[k-1]+0.5);
				xz2=(int)(kx[k2-1]+0.5);
				yz=(int)(ky[k-1]+0.5);
				yz2=(int)(ky[k2-1]+0.5);
				Point supertemp = new Point(xz,yz);
				Point supertemp2 = new Point (xz2, yz2);
				boolean inside = false;
				if(!
				   (
				    supertemp.getX()==0||
				    supertemp.getX()==haba||
				    supertemp.getY()==0||
				    supertemp.getY()==taka)&&!
				   (
				    supertemp2.getX()==0||
				    supertemp2.getX()==haba||
				    supertemp2.getY()==0||
				    supertemp2.getY()==taka
				    )
				   ){
				    for( int j = 0; j < obsCount; j++ ) {
					Obstacle o = w.getObstacle(j);
					Polygon p  = o.getPoly();
					if(p.contains(supertemp)||p.contains(supertemp2)) {
					    inside = true;
					    //System.out.println("Inside is true!");
					}
				    }
				}
				else
				    inside = true;
				if( !inside && !intersect(supertemp,supertemp2,w))
				{
				    whiteStart.add(supertemp);
				    whiteEnd.add(supertemp2);
				}
				// System.out.println("Drawing line");
				//g.drawLine(xz,yz,xz2,yz2);  // Draw lines, maybe trap pts here?
				//g.fillOval(xz,yz,4,4); 
				//g.fillOval(xz2,yz2,4,4);
				break;
			    }//if br2==0
			}//next k
		    }//if br==0
		}//next j
	    }//next i
	}
	/*
	int size = whiteStart.size();
	Point ultratemp;
	int asdf,qwerty;
	boolean found;

	for(asdf=0; asdf < size; asdf++) {
	    ultratemp = (Point) whiteStart.get(asdf);
	    found = false;
	    for(qwerty=0; qwerty < size && !found; qwerty++) {
		if( ultratemp.equals( (Point) whiteStart.get(qwerty) ) || ultratemp.equals( (Point) whiteEnd.get(qwerty) ) ) {
		    found = true;
		    break;
		}
	    }
	    if(!found) {
		whiteStart.remove(asdf);
		whiteEnd.remove(asdf);
	    }
	}
	

	for(asdf=0; asdf < size; asdf++) {
	    ultratemp = (Point) whiteEnd.get(asdf);
	    found = false;
	    for(qwerty=0; qwerty < size && !found; qwerty++) {
		if( ultratemp.equals( (Point) whiteStart.get(qwerty) ) || ultratemp.equals( (Point) whiteEnd.get(qwerty) ) ) {
		    found = true;
		    break;
		}
	    }
	    if(!found) {
		whiteStart.remove(asdf);
		whiteEnd.remove(asdf);
	    }
	}
	*/
	
				
	// ((SolutionPath)paths.get(0)).showValues(); 

	    return paths;
    }
    
    
    
    public double dou(String dous){
	double dou1;
	dou1 = (Double.valueOf(dous)).doubleValue();
	return dou1;
    }
 
    public void init(Scenario problem){
	System.out.println("init()");
	w = problem.getWorldspace();

	haba=w.getDimX();
	taka=w.getDimY();

	obsCount = w.getNumObstacles();
	ipts = new Vector();
	tempPts = new Vector();
	nextPt = 0;
	ptCount = 0;
	
	for (int i=0; i < problem.getNRobots(); i++) {
	    Point prevPoint, currentPoint, firstPoint;

	    for( int j = 0; j < obsCount; j++ )
		{
		    Obstacle o = w.getObstacle(j);
		    //Polygon p = o.getPoly();
		    int vertCount = o.getNumVertices();
		    int ron = 1;
		    prevPoint = o.getPoint(0);
		    firstPoint = prevPoint;
		    currentPoint = firstPoint;
		    for( ron = 1; ron < vertCount; ron++ ) 
			{
			    currentPoint = o.getPoint(ron);
			    pstar.add(prevPoint);
			    addpoints(prevPoint, currentPoint, pstar);
			    ptCount++;
			    // System.out.println("currentPoint="+currentPoint);
		   
			    prevPoint = currentPoint;
			}
		    addpoints(currentPoint, firstPoint, pstar);
		    pstar.add(currentPoint);
		}
	    for( int j = 0; j < taka; j+=25)
		{
		    Point cxcz = new Point(0, j);
		    Point czcx = new Point(haba, j);
		    pstar.add(cxcz);
		    pstar.add(czcx);
		}

	    for( int j = 0; j < haba; j+=25)
		{
		    Point cxcz = new Point(j, 0);
		    Point czcx = new Point(j, taka);
		    pstar.add(cxcz);
		    pstar.add(czcx);
		}
	}

	int size = pstar.size();

	for(int ron = 0; ron < size; ron++) {
	    //System.out.println("Copying point " +(Point) pstar.get(ron));
	    p[ron] = (Point) pstar.get(ron);
	    ptCount = ron;
	}
	
	n=ptCount;
	col1=Color.red;
	col2=Color.yellow;
	col3=Color.white;
    }
    
    public void addpoints(Point a, Point b, Vector V)
    {
	//	System.out.println("addpoints. a="+a+" b="+b);
	double aX = a.getX();
	double aY = a.getY();
        double bX = b.getX();
	double bY = b.getY();
	double changeX = aX - bX;
	double changeY = aY - bY;
	int distance = (int) Math.sqrt(changeX * changeX + changeY * changeY);
	int numSteps = (int)distance/10;
	for(int i = 1; i < numSteps; i++)
	{
	    Point veryTempPoint = new Point((int) (aX * i / numSteps + bX * (numSteps - i) / numSteps),
					    (int) (aY * i / numSteps + bY * (numSteps - i) / numSteps));
	    V.add(veryTempPoint);
	}
	return;
    }

    public double jou(double a,double b){
	double jou1;
	jou1=Math.pow(a,b);
	return jou1;
    }
    void heapv(double te1[],double te2[],double te3[],int NN){
	// System.out.println("heapv()");
	int kk,kks,ii,jj,mm;
	double b1,b2,b3,c1,c2,c3;
	kks=(int)(NN/2);
	for(kk=kks;kk>=1;kk--){
	    ii=kk;
	    b1=te1[ii-1];b2=te2[ii-1];b3=te3[ii-1];
	    while(2*ii<=NN){
		jj=2*ii;
		if(jj+1<=NN){
		    if(te1[jj-1]<te1[jj]){
			jj++;
		    }
		}
		if(te1[jj-1]<=b1){
		    break;
		}
		te1[ii-1]=te1[jj-1];te2[ii-1]=te2[jj-1];te3[ii-1]=te3[jj-1];
		ii=jj;
	    }//while-end
	    te1[ii-1]=b1;te2[ii-1]=b2;te3[ii-1]=b3;
	}//next kk
	for(mm=NN-1;mm>=1;mm--){
	    c1=te1[mm];c2=te2[mm];c3=te3[mm];
	    te1[mm]=te1[0];te2[mm]=te2[0];te3[mm]=te3[0];
	    ii=1;
	    while(2*ii<=mm){
		kk=2*ii;
		if(kk+1<=mm){
		    if(te1[kk-1]<=te1[kk]){
			kk++;
		    }
		}
		if(te1[kk-1]<=c1){
		    break;
		}
		te1[ii-1]=te1[kk-1];te2[ii-1]=te2[kk-1];te3[ii-1]=te3[kk-1];
		ii=kk;
	    }//while-end
	    te1[ii-1]=c1;te2[ii-1]=c2;te3[ii-1]=c3;
	}//next mm
    }
    
    public void draw(Graphics g){
	//		System.out.println("draw()");
	//	g.setColor(col1);
	//g.fillRect(1,1,haba,taka);
	g.setColor(col2);
	
	int size = yellow.size();

	/*	for(int ron = 0; ron < size; ron++) {
	    Point tempPt = (Point)yellow.get(ron);
	    g.fillOval((int)tempPt.getX(),(int)tempPt.getY(),4,4); 
	    }	*/

	g.setColor(col3);
	size = whiteStart.size();

	for(int ron = 0; ron < size; ron++) {
	    Point tempPt = (Point)whiteStart.get(ron);
	    Point tempPt2 = (Point)whiteEnd.get(ron);
	
	    /*   g.fillOval((int)tempPt.getX(),(int)tempPt.getY(),4,4); 
		 g.fillOval((int)tempPt2.getX(),(int)tempPt2.getY(),4,4); */
	    g.drawLine((int)tempPt.getX(),(int)tempPt.getY(),(int)tempPt2.getX(),(int)tempPt2.getY());	    
	}

	g.drawString("N="+N,15,15);
    }



    /** 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(closeToA) || temp.contains(closeToB))
		//    return true;
		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))
			    {
				//	System.out.println("returning from isOn now...as true");
				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;
    }


    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;
    }

}