import java.awt.*;
import java.util.Date;
import java.util.Vector;
import java.util.Iterator;

public class SwarmPathDraw implements ISimUICallback, ISwarmDraw
{

    long m_nDrawEveryMilli;
    double m_lfStoreEveryMilliReal;
    double m_lfStoreEverySim;
    double m_lfLastSimStored;
    Date m_lastDateDrawn;
    Date m_dateZeroReference;
    double m_lfSecRealPerSecSim;
    double m_lfSecSimPerSecReal;
    
    boolean m_bDrawVoronoi; // only works if there is actually voronoi
    // information attached to edges

    IProjection m_xformToScreen;
    
    IEnvironment m_env;
    IEnvDraw m_envDraw;

    SwarmState m_stateToDraw;
    SynchronousStateQueue m_stateQueue;

    int m_nFrameCacheSize;

    Vector<double[]> m_vectorPosState;

    CommGraph m_commGraph;

    boolean m_bDrawAllLinks;

    int m_nPathSampFreq;
    int m_nPathSampCnt;

    SwarmPathDraw()
    {
	m_xformToScreen = null;

	m_nPathSampCnt = 0;
	m_nPathSampFreq = 5;

	m_nDrawEveryMilli = 100;
	m_lfSecRealPerSecSim = 1.0;
	m_lfSecSimPerSecReal = 1.0/m_lfSecRealPerSecSim;
	m_dateZeroReference = new Date();
	m_lastDateDrawn = m_dateZeroReference;
	m_stateToDraw = null;
	m_stateQueue = new SynchronousStateQueue();
	m_lfStoreEveryMilliReal = 100.0;
	m_lfStoreEverySim = m_lfSecSimPerSecReal*0.001*
	    m_lfStoreEveryMilliReal;
	m_lfLastSimStored = -m_lfStoreEverySim-1.0;

	m_vectorPosState = new Vector<double[]>();

	m_nFrameCacheSize=60;
    
	m_commGraph = null;

	m_bDrawAllLinks = true;
	
	m_bDrawVoronoi = false;
    }
    
    public void setDrawVoronoi(boolean bDrawVoronoi) {
    	m_bDrawVoronoi = bDrawVoronoi;
    }

    public Date simTimeToRealTime(double lfSimTime)
    {
	return new Date(m_dateZeroReference.getTime() +
			(long)(m_lfSecRealPerSecSim*lfSimTime*1000));
    }

    public double realTimeToSimTime(Date dateRealTime)
    {
	return (double)(dateRealTime.getTime()-
			m_dateZeroReference.getTime())*
	    0.001*m_lfSecSimPerSecReal;
    }

    public void setDrawAllLinks(boolean bDrawAllLinks) {
	m_bDrawAllLinks = bDrawAllLinks;
    }

    /**
     * does nothing in this implementation
     */
    public void setStaticEnvironment(IEnvironment env)
    { m_env = env.makeCopy(); }

    /**
     * does nothing in this implementation
     */
    public synchronized void setDynamicEnvironment(IEnvironment env)
    { m_env = env.makeCopy(); }


    public void setEnvDraw(IEnvDraw envDraw) {
    	m_envDraw = envDraw;
    }
    
    Vector<IDrawAgent> m_vecAgents;
    {m_vecAgents = new Vector<IDrawAgent>(); }


    public void addDrawAgent(IDrawAgent agent) {
    	m_vecAgents.add(agent);
    }
    
    /**
     * Pass in a projection class to handle mapping points to
     * screen coords.
     */
    public void setProjection(IProjection proj)
    {
	m_xformToScreen = proj;
    }
    
    public void setSampFreq(int nSampFreq) {
    	m_nPathSampFreq = nSampFreq;
    }
    
    public void scaleProjection(int nNewX, int nNewY) {}
    
    public void xlateProjection(int nXOff, int nYOff) {}


    public void reDraw(java.awt.Graphics g)
    {

	Date currTime = new Date();
	
	if(currTime.getTime() - m_lastDateDrawn.getTime() > 
	   m_nDrawEveryMilli)
	    {

		if(m_nPathSampCnt % m_nPathSampFreq == 0) {
		    m_stateToDraw = m_stateQueue.getLeastRecent(m_stateToDraw);
		    if(m_stateToDraw != null) {
			double [] arrLfPoints = 
			    m_stateToDraw.accessPositions();
			m_vectorPosState.add(arrLfPoints);
		    }
		}
		++m_nPathSampCnt;
		
		m_lastDateDrawn = currTime;
		m_stateQueue.removeBefore(realTimeToSimTime(currTime));
	    }  

	
	m_stateToDraw = m_stateQueue.getLeastRecent(m_stateToDraw);
	

	if(m_stateToDraw == null)
	    {
		return;
	    }

	double [] arrLfPoints = m_stateToDraw.accessPositions();
	StateBundle [] arrAgentStates = m_stateToDraw.accessStates();

	CommGraph currGraph = m_stateToDraw.getGraph();

	if(m_xformToScreen == null) {
	    g.setColor( globalsUI.getColorMap().getColor(0));
	    g.fillRect(0,0,20000,20000);
	 
	    if(m_envDraw != null) {
	    	m_envDraw.draw(m_env, m_xformToScreen, g);
	    }
	    
	    for(int nDrwAgnt = 0; nDrwAgnt < m_vecAgents.size(); ++nDrwAgnt) {
	    	IDrawAgent drawAgent = m_vecAgents.elementAt(nDrwAgnt);
	    	drawAgent.draw(g, m_xformToScreen,arrLfPoints,arrAgentStates,
	    			currGraph);
	    }
	    g.setColor( globalsUI.getColorMap().getColor(1) );

	    
	    g.setColor(globalsUI.getColorMap().getColor(2));
	    g.drawOval(50,50,10,10);
	    if(arrLfPoints != null) {
		for(int i = 0; i < arrLfPoints.length; i += 2)
		    {
			g.setColor(globalsUI.getColorMap().getColor(3));
			g.drawOval((int)arrLfPoints[i], 
				   (int)arrLfPoints[i+1], 
				   10, 10);
		    }
	    }
	} 
	else {

	    g.setColor( globalsUI.getColorMap().getColor(0));
	    g.fillRect(m_xformToScreen.left(),
		       m_xformToScreen.top(),
		       m_xformToScreen.left() + m_xformToScreen.width(), 
		       m_xformToScreen.top() + m_xformToScreen.height());
	    
	    
	    if(m_envDraw != null) {
	    	m_envDraw.draw(m_env, m_xformToScreen, g);
	    }
	    
	    for(int nDrwAgnt = 0; nDrwAgnt < m_vecAgents.size(); ++nDrwAgnt) {
	    	IDrawAgent drawAgent = m_vecAgents.elementAt(nDrwAgnt);
	    	drawAgent.draw(g, m_xformToScreen,arrLfPoints,arrAgentStates,
	    			currGraph);
	    }
	    
	    g.setColor( globalsUI.getColorMap().getColor(1) );

	    
	    if(arrLfPoints != null) {
		int nRad = 10;
		try {
		    nRad = m_xformToScreen.scaleRadius(0.01);
		} catch(Exception e) {
		    double [] lfTmp = new double [2];
		    lfTmp[0] = 7.07; lfTmp[1] = 7.07;
		    int [] nTmp = m_xformToScreen.scaleVector(lfTmp, 0);
		    nRad = (int)Math.sqrt(nTmp[0]*nTmp[0]+nTmp[1]*nTmp[1]);
		}

		if(nRad < 1) { nRad = 10; }

		IProjection p = m_xformToScreen;
		double [] pts = arrLfPoints;
		int nPts = pts.length/2;
		// so lines don't run over 80 characters
		
		if(currGraph != null) {

		    
		    
		    for(int i = 0; i < nPts; ++i) {
		    	g.setColor(globalsUI.getColorMap().getColor(4));
			g.drawOval(p.getScreenComp(0, pts, 2*i)-nRad*2,
				   p.getScreenComp(1, pts, 2*i)-nRad*2,
				   nRad*4, nRad*4);
			if(m_bDrawAllLinks) {
				g.setColor(globalsUI.getColorMap().getColor(2));
			    Iterator<CommLink> iterTo = currGraph.getEdgesTo(i);
			    while(iterTo.hasNext()) {
				CommLink l = iterTo.next();
				g.drawLine(p.getScreenComp(0,pts, 2*l.to()),
					   p.getScreenComp(1,pts, 2*l.to()),
					   p.getScreenComp(0,pts, 2*l.from()),
					   p.getScreenComp(1,pts, 2*l.from()));
			    
//			    g.drawOval(
//			    		(p.getScreenComp(0, pts, 2*l.to())+
//			    				3*p.getScreenComp(0,pts,2*l.from()))/4
//			    		-nRad*1,
//			    		(p.getScreenComp(1, pts, 2*l.to())+
//			    				3*p.getScreenComp(1,pts,2*l.from()))/4
//			    		-nRad*1,
//						   nRad*2, nRad*2);
			    }
			
			    if(m_bDrawVoronoi) {
			    	g.setColor(globalsUI.getColorMap().getColor(3));
			    	Iterator<CommLink> iterFrom = currGraph.getEdgesFrom(i);
			    	try {
			    		if(i%3 == 0) {
			    			g.setColor(globalsUI.getColorMap().getColor(1));
			    		} else if(i%3 == 1) {
			    			g.setColor(globalsUI.getColorMap().getColor(5));
			    		} else {
			    			g.setColor(globalsUI.getColorMap().getColor(3));
			    		}
			    		while(iterFrom.hasNext()) {
			    			CommLink l = iterFrom.next();
			    			if(l != null) {
			    				CommLink.IChannelData chanData = 
			    					l.getChannelData();
			    				if(chanData == null) {
			    					continue;
			    				}
			    				DistributedDelaunayClip.Edge
			    				currEdge = 
			    					(DistributedDelaunayClip.Edge)chanData;
			    				if(!currEdge.edgeValid()) {
			    					continue;
			    				}
			    				double [] arrLfEnd = 
			    					currEdge.getEdgeEndClip(.5, 1);
			    				double [] arrLfStart = 
			    					currEdge.getEdgeStartClip(.5, 1);
			    				double [] arrLfClosePt = currEdge.getClosestPt();
			    				
			    				double lfClosePtMagSqrd = 
			    					arrLfClosePt[0]*arrLfClosePt[0] +
			    					arrLfClosePt[1]*arrLfClosePt[1];
			    				
			    				double lfNeighborMagSqrd = 
			    					(pts[2*l.from()] - pts[2*l.to()]);
			    				double lfTmp =  
			    					(pts[2*l.from()+1] - pts[2*l.to()+1]);
			    				
			    				lfNeighborMagSqrd = lfTmp*lfTmp +
			    				lfNeighborMagSqrd*lfNeighborMagSqrd;
			    				
			    				if(lfClosePtMagSqrd > lfNeighborMagSqrd) {
			    					g.setColor(globalsUI.getColorMap().getColor(4));
			    				}
			    				
			    				double lfCheckEnd = arrLfEnd[0]*arrLfClosePt[0] +
			    				arrLfEnd[1]*arrLfClosePt[1] -
			    				arrLfClosePt[0]*arrLfClosePt[0] -
			    				arrLfClosePt[1]*arrLfClosePt[1];
				
			    				double lfCheckStart = arrLfStart[0]*arrLfClosePt[0] +
			    				arrLfStart[1]*arrLfClosePt[1] -
			    				arrLfClosePt[0]*arrLfClosePt[0] -
			    				arrLfClosePt[1]*arrLfClosePt[1];
			    				
			    				if(Math.abs(lfCheckEnd) > 0.001) {
			    					System.out.println("Endpoint doesn't check out");
			    				}
				
			    				if(Math.abs(lfCheckStart) > 0.001) {
			    					System.out.println("Startpoint doesn't check out");
			    				}
			    				
			    				double lfScaleVC = 0.98;
			    				
			    				arrLfEnd[0] = lfScaleVC*arrLfEnd[0] + 
			    					pts[2*l.from()];
			    				arrLfEnd[1] = lfScaleVC*arrLfEnd[1] + 
			    					pts[2*l.from()+1];
			    				arrLfStart[0] = lfScaleVC*arrLfStart[0] + 
			    					pts[2*l.from()];
			    				arrLfStart[1] = lfScaleVC*arrLfStart[1] + 
		    						pts[2*l.from()+1];
			    				// 	arrLfClosePt[0] *= 2;
			    				// 	arrLfClosePt[1] *= 2;
			    				arrLfClosePt[0] += pts[2*l.from()];
			    				arrLfClosePt[1] += pts[2*l.from()+1];
			    				g.drawLine(p.getScreenComp(0,arrLfStart, 0),
			    						p.getScreenComp(1,arrLfStart, 0),
			    						p.getScreenComp(0,arrLfEnd, 0),
			    						p.getScreenComp(1,arrLfEnd, 0));
			    				/*
			    				g.drawLine(p.getScreenComp(0,arrLfStart, 0),
			    						p.getScreenComp(1,arrLfStart, 0),
			    						p.getScreenComp(0,pts, 2*l.from()),
			    						p.getScreenComp(1,pts, 2*l.from()));
			    				
			    				g.drawLine(p.getScreenComp(0,pts, 2*l.from()),
			    						p.getScreenComp(1,pts, 2*l.from()),
			    						p.getScreenComp(0,arrLfEnd, 0),
			    						p.getScreenComp(1,arrLfEnd, 0));
			    				
			    				g.drawOval(p.getScreenComp(0, arrLfClosePt, 0)-nRad,
			    						p.getScreenComp(1, arrLfClosePt, 0)-nRad,
			    						2*nRad, 2*nRad);
			    				
			    				g.drawOval(p.getScreenComp(0,arrLfStart, 0)-nRad,
			    						p.getScreenComp(1,arrLfStart, 0)-nRad,
			    						2*nRad,2*nRad);
			    				*/
			    				double lfXAvg = pts[2*l.from()] + pts[2*l.to()];
			    				lfXAvg *= 0.5;
			    				double lfYAvg = pts[2*l.from()+1] + pts[2*l.to()+1];
			    				lfYAvg *= 0.5;
			    				
			    				if(Math.abs(lfXAvg - arrLfClosePt[0]) > 0.1 ||
			    						Math.abs(lfYAvg - arrLfClosePt[1]) > 0.1) {
			    					System.out.println("Problem");
			    					
			    				}
			    				
			    				
			    				
			    			}
			    		}
			    	} catch(java.lang.ClassCastException e) {
			    		System.out.println("Couldn't draw");
			    	}
			    }
		    	}
		    }
		}
		
		g.setColor(globalsUI.getColorMap().getColor(1));
		for(int i = 0; i < arrLfPoints.length; i += 2) {
		    
		    g.drawOval(m_xformToScreen.getScreenComp(0,arrLfPoints,i)
			       -nRad,
			       m_xformToScreen.getScreenComp(1,arrLfPoints,i)
			       -nRad,
			       2*nRad, 2*nRad);
		    

		}

		drawBorder(g);
		drawLabel(g);


		g.setColor(globalsUI.getColorMap().getColor(5));
		
		Iterator<double []> posIter = m_vectorPosState.iterator();

		double arrLfPosLast[] = null;
		if(posIter.hasNext()) {
		    arrLfPosLast = 
			(double [])posIter.next();
		}
		while(posIter.hasNext()) {
		    double arrLfPosSnap[] = 
			(double [])posIter.next();
		    
		    if(arrLfPosLast == null || arrLfPosSnap == null) {
	    		continue;
	    	}
		    for(int i = 0; i < arrLfPosSnap.length; i += 2) {
		    	
			g.drawLine(p.getScreenComp(0, arrLfPosLast, i),
				   p.getScreenComp(1, arrLfPosLast, i),
				   p.getScreenComp(0, arrLfPosSnap, i),
				   p.getScreenComp(1, arrLfPosSnap, i));
			
		    }
		    arrLfPosLast = arrLfPosSnap;
		}


		if(false) { // for debugging only
		    try {
			for(int i = 0; i < nPts; ++i ) {
			    ControlFuncMoveTowards func =
				(ControlFuncMoveTowards)
				arrAgentStates[i].getControlFunc();
			    if(arrAgentStates[i].getVars().getBoolVar(new
								      Integer(1)).
			       booleanValue()
			       ) {
				g.setColor(globalsUI.getColorMap().getColor(4));
			    } else {
				g.setColor(globalsUI.getColorMap().getColor(5));
			    }
			    g.drawLine(p.getScreenComp(0, pts, 2*i),
				       p.getScreenComp(1, pts, 2*i),
				       p.getScreenComp(0, func.getTowards(),0),
				       p.getScreenComp(1, func.getTowards(),0));
			}
		    } catch(java.lang.ClassCastException e) {
		    }
		}

		
	    }
	} 
    }
    
    String m_strLabel = "Paths";
    
    public void setLabel(String strLabel) {
    	m_strLabel = strLabel;
    }
    
    void drawLabel(java.awt.Graphics g) {
    	g.setColor(globalsUI.getColorMap().getColor(5));
    	String str = m_strLabel; //new String("Paths");
    	globalsUI.getInstance().doText(g, str,
    			m_xformToScreen.left() + 
    			2*globalsUI.getInstance().getLineThickness(), 
    			m_xformToScreen.top()+m_xformToScreen.height()-
    			2*globalsUI.getInstance().getLineThickness(), -1);
    }

    void drawBorder(java.awt.Graphics g) {
    	g.setColor(globalsUI.getColorMap().getColor(7));
    	g.drawLine(m_xformToScreen.left(),
 		       m_xformToScreen.top(),
 		       m_xformToScreen.left() + m_xformToScreen.width()-1, 
 		       m_xformToScreen.top());
    	g.drawLine(m_xformToScreen.left(),
  		       m_xformToScreen.top()+ m_xformToScreen.height()-1,
  		       m_xformToScreen.left() + m_xformToScreen.width()-1, 
  		       m_xformToScreen.top()+ m_xformToScreen.height()-1);
    	g.drawLine(m_xformToScreen.left() + m_xformToScreen.width()-1,
   		       m_xformToScreen.top(),
   		       m_xformToScreen.left() + m_xformToScreen.width()-1, 
   		       m_xformToScreen.top()+ m_xformToScreen.height()-1);
    	g.drawLine(m_xformToScreen.left(),
    		       m_xformToScreen.top(),
    		       m_xformToScreen.left(), 
    		       m_xformToScreen.top()+ m_xformToScreen.height()-1);
    }

    public void paint(java.awt.Graphics g) {
	reDraw(g);
    }
    

    /**
     * Callback function
     * @param arrLfState state vector (initially initial state,
     * overwritten with final state)
     * @param agent agent mapping (currently only support for homogenous swarms
     * )
     * @param arrStates discrete state and control function 
     * of each agent (assumed
     * not to change over integration)
     * @param arrSensors array of sensor interfaces.
     * @param env environment
     * @return true if something unusual has happened
     */
    public boolean doCallback(double [] arrLfState,
			      IAgent agent,
			      StateBundle [] arrStates,
			      ISensor [] arrSensors,
			      IEnvironment env,
			      CommGraph graph,
			      double lfSimTime)
    {
	/*     long m_nDrawEveryMilli;
	       double m_lfStoreEveryMilliReal;
	       double m_lfStoreEverySim;
	       Date m_lastDateDrawn;
	       Date m_dateZeroReference;
	       double m_lfSecRealPerSecSim;
	       double m_lfSecSimPerSecReal;
	       
	       double m_lfLastSimStored;
	       IProjection m_xformToScreen;
	       
	       SwarmState m_stateToDraw;
	       SynchronousStateQueue m_stateQueue;
	*/

	if(graph != null) {
	    m_commGraph = new CommGraph(graph);
	}

	if(lfSimTime - m_lfLastSimStored > m_lfStoreEverySim) 
	    {
		m_lfLastSimStored = lfSimTime;
		double lfTimeAheadBy = 
		    m_stateQueue.addState(arrLfState,
					  arrStates, agent, lfSimTime,
					  m_commGraph);
		if(lfTimeAheadBy > 
		   m_nFrameCacheSize*m_nDrawEveryMilli*0.001) 
		    {
			try {
			    Thread.sleep(m_nDrawEveryMilli);
			}
			catch(InterruptedException e)
			    {
			    }

		    }
		
	
	    }
	return false;
    }

    /**
     * In case we need more info then true/false from doCallback
     * @return null
     */
    public Object getLastObject()
    {
	return null;
    }

}