import java.util.Comparator;
import java.util.Iterator;
import java.util.Map;
import java.util.TreeMap;
import java.util.Vector;
import java.util.Stack;





/**
 * <p>
 * Allows an IInitializer to generate a specification of a
 * target configuration suitable for formation morphing algorithms.
 * </p>
 * @author     Michael Schuresko
 * @version    %I%, %G%
 * @since      1.1
 */

public class MorphTargetAdapter {

    IInitializer m_init;
    double m_lfConnectRad;

    public MorphTargetAdapter() {
	m_init = null;
	m_lfConnectRad = 1.0;
    }

    public MorphTargetAdapter(IInitializer init) {
	m_init = init;
	
    }
    
    public MorphTargetAdapter(IInitializer init, double lfConnectRad) {
    	m_init = init;
    	m_lfConnectRad = lfConnectRad;
    }
    
    public void setConnectRad(double lfConnectRad) {
    	m_lfConnectRad = lfConnectRad;
    }

    public void setInitializer(IInitializer init) {
	m_init = init;
    }

    public class VecComp implements Comparator<Vector<Double>>
    {
    	public int compare(Vector<Double> v1, Vector<Double> v2) {
    		if(v1.get(0) < v2.get(0)) {
    			return -1;
    		}
    		if(v1.get(0) == v2.get(0)) {
    			return 0;
    		}
    		return 1;
    	}
    	
    	public boolean equals(Vector<Double> v1, Vector<Double> v2) {
    		return (v1.get(0) == v2.get(0));
    	}
    }
    
    public class VecFromArray {
    	double [] m_arrLfLoc;
    	int m_nDim;
    	
    	public VecFromArray(double [] arrLfLoc, int nDim) {
    		m_arrLfLoc = arrLfLoc;
    		m_nDim = nDim;
    	}
    	
    	public Vector<Double> getByOffset(int nIdxOff) {
    		Vector<Double> vecOut = new Vector<Double>(m_nDim);
    		for(int i = 0; i < m_nDim; ++i) {
    			vecOut.add(i,m_arrLfLoc[nIdxOff+i]);
    		}
    		return vecOut;
    	}
    	
    	public Vector<Double> getByAgent(int nIdxAgent) {
    		return getByOffset(nIdxAgent*m_nDim);
    	}
    }
    
    public IMorphSpec generate() {
    	MorphSpecStoredVals specOut = 
    		new MorphSpecStoredVals();
    	double arrLfLocs[] = 
    		m_init.getInitContState();
    	int nDim = m_init.getEnv().getDimensionality();
    	int nNumAgents = arrLfLocs.length/nDim;

    	double arrLfTmpLoc[] = new double[nDim];


    	TreeMap<Vector<Double>, Integer> mapPos =
    		new TreeMap<Vector<Double>, Integer>(new VecComp());
    	
    	
    	
    	Stack<Map.Entry<Vector<Double>, Integer> > stackAgents =
    		new Stack<Map.Entry<Vector<Double>, Integer> >();
    	
    	VecFromArray vecFac = new VecFromArray(arrLfLocs, nDim);
    	
    	boolean arrBFound[] = new boolean[nNumAgents];
    	
    	int i = 0;
    	int nAxis = 0;
    	for(i = 0; i < nNumAgents; ++i) {
    		Vector<Double> vecKey = vecFac.getByAgent(i);
    		mapPos.put(vecKey, i);
    		
    		arrBFound[i] = false;
    	}
    	
    	
    	
    	int nParent = 0;
    	
    	Iterator<Map.Entry<Vector<Double>, Integer> > iMapIter =
			mapPos.entrySet().iterator();
		
		stackAgents.push(iMapIter.next());
		
    	
    	while(! stackAgents.isEmpty()) {
    		Map.Entry<Vector<Double>, Integer> popped = stackAgents.pop();
    		arrBFound[popped.getValue()] = true;
    		
    		if(popped.getValue() == 0) {
    			for(nAxis = 0; nAxis < nDim; ++nAxis) {
					arrLfTmpLoc[nAxis] = popped.getKey().get(nAxis);
				}
    			specOut.addAgent(arrLfTmpLoc, nParent);
    		}
    		Vector<Double> vecMinRange = new Vector<Double>(popped.getKey());
    		Vector<Double> vecMaxRange = new Vector<Double>(popped.getKey());
    		
    		Vector<Double> vecCurrPos = popped.getKey();
    		
    		vecMinRange.set(0, vecMinRange.get(0) - m_lfConnectRad);
    		vecMaxRange.set(0, vecMaxRange.get(0) + m_lfConnectRad);
    		
    		Iterator<Map.Entry<Vector<Double>, Integer> > iPosPairs = 
    			mapPos.subMap(vecMinRange, vecMaxRange).entrySet().iterator();
    		
    		while(iPosPairs.hasNext()) {
    			Map.Entry<Vector<Double>, Integer> currEntry = 
    				iPosPairs.next();
    			Integer nObjRep = currEntry.getValue();
    			Vector<Double> vecPosCmp = currEntry.getKey();
    			double lfDistSum = 0.0;
    			for( nAxis = 0; nAxis < nDim; ++nAxis) {
    				double lfAxisDist = vecCurrPos.get(nAxis) -
    					vecPosCmp.get(nAxis);
    				lfDistSum += lfAxisDist*lfAxisDist;
    			}
    			
    			if(lfDistSum < m_lfConnectRad*m_lfConnectRad && nObjRep.intValue() != i) {
    				if(arrBFound[nObjRep.intValue()] != true) {
    					for(nAxis = 0; nAxis < nDim; ++nAxis) {
    						arrLfTmpLoc[nAxis] = vecPosCmp.get(nAxis);
    					}
    					
    					specOut.addAgent(arrLfTmpLoc, nParent);
    					stackAgents.push(currEntry);
    					arrBFound[nObjRep.intValue()] = true;
    				}

    			}
    		}
    		
    		++nParent;
    	}

    	specOut.convertVals();
    	return specOut;
    }
}