import java.util.Comparator;
import java.util.Arrays;

/**
 * <p>
 * Proximity graph function for r-disk graph.
 * </p>
 * @author     Michael Schuresko
 * @version    %I%, %G%
 * @since      1.0
*/

public class RdiskGraph implements IProxGraph 
{
    double m_lfRad;
    
    /**
     * preferred constructor
     * @param lfRadius puts the "r" in "r-disk graph"
     */
    public RdiskGraph(double lfRadius)
    {
	m_lfRad = lfRadius;
    }

    public RdiskGraph()
    {
	m_lfRad = 1.0;
    }

    public class SortHelper implements Comparator<Integer> {
	public double [] m_arrLfPositions;
	public int m_nDim;
	public int m_nSortOff;

	public SortHelper(double arrLfPositions[], int nDim, int nSortOff) {
	    m_nDim = nDim;
	    m_nSortOff = nSortOff;
	    m_arrLfPositions = arrLfPositions;
	}

	public int compare(Integer nObj1, Integer nObj2) {
	    int n1 = m_nDim*((Integer)nObj1) + m_nSortOff;
	    int n2 = m_nDim*((Integer)nObj2) + m_nSortOff;
	    if(m_arrLfPositions[n1] < 
	       m_arrLfPositions[n2]) {
		return -1;
	    } else if(m_arrLfPositions[n2] < 
		      m_arrLfPositions[n1]) {
		return 1;
	    }
	    return 0;
	}

	public boolean equals(Object obj) {
	    try {
		SortHelper castObj = (SortHelper)obj;
		return (castObj.m_arrLfPositions == m_arrLfPositions &&
			castObj.m_nDim == m_nDim && 
			castObj.m_nSortOff == m_nSortOff);
	    } catch(ClassCastException e) {
		return false;
	    }
	}

    }




    /**
     * Proximity graph function: maps finite collects of points
     * in R^d onto graphs.  Copies communication channels from
     * previous graph, if available.  Does not know about
     * agent discrete state or other internal state vars.
     * Assumes that the first d entries in the dimension array correspond
     * to point 0, the next d to point 1, the next d to point 2 etc.
     * Must be able to work without an optimization helper passed in.
     * (this version is completely unoptimized -- fix later)
     * @param nDimension the dimensionality the points live in, is this a graph in r^2, r^3, etc?
     * @param arrPoints position vector
     * @param optHelper optimization helper, assumed to be modified 
     * (be sure to duplicate before passing in)
     * modification of this parameter is a way of returning a helper value.
     * @param prevGraph copy message queues (including unread messages) 
     * from prevGraph if possible. Most not be null!
     * @param env current environment.
     * @return new graph corresponding to this set of points in space.
     */
    public CommGraph getGraph(int nDimension, double arrPoints[],
			      IProxGraphOptWrapper optHelper,
			      CommGraph prevGraph,
			      IEnvironment env)
    {
	CommGraph gResult = new CommGraph(prevGraph.getSize());
	
	gResult.setChannelType(prevGraph.getChannelType());

	int nDim = env.getDimensionality();

	Integer arrOffsets[] = new Integer[arrPoints.length / nDimension];
	for(int i = 0; i < arrOffsets.length; ++i) {
	    arrOffsets[i] = new Integer(i);
	}

	SortHelper compHelper = new SortHelper(arrPoints, nDimension, 0);

	Arrays.sort(arrOffsets, compHelper);

	for(int i = 0; i < arrOffsets.length; ++i) {
	    for(int j = i; j<arrOffsets.length &&
		    arrPoints[arrOffsets[j]*nDimension] <=
		    arrPoints[arrOffsets[i]*nDimension]+m_lfRad; ++j) {
		
		if(distHelper(arrPoints, nDim, arrOffsets[i], arrOffsets[j]) <
		   m_lfRad*m_lfRad) {
		    gResult.addLink(arrOffsets[i],arrOffsets[j], prevGraph);
		    gResult.addLink(arrOffsets[j],arrOffsets[i], prevGraph);
		}
	    }
	}

	return gResult;
    }


    /**
     * distance helper function
     * @param nDim dimensionality of space points are embedded in
     * @param nIdx1 index of the first point (offset is nDim*nIdx1)
     * @param nIdx2 index of the second point (offset is nDim*nIdx2)
     * @return squared Euclidean distance between robots 1 and 2
     */
    double distHelper(double arrPoints[],
		      int nDim, int nIdx1, int nIdx2)
    {
	double lfResult = 0.0;
	for(int i = 0; i < nDim; ++i) {
	    double lfTmp = arrPoints[nDim*nIdx1+i] -
		arrPoints[nDim*nIdx2+i];
	    lfResult = lfResult + lfTmp*lfTmp;
	}
	return lfResult;
    }
}