/**
 * <p>
 * Messages sent by agents following tree connectivity protocol.
 * </p>
 * @author     Michael Schuresko
 * @version    %I%, %G%
 * @since      1.0
*/
public class TreeMsg implements IMsg
{
    TreeAgentState m_treeState;

    double [] m_arrLfParentDir;
    double [] m_arrLfPerpDir;
    double [] m_arrLfLoc;

    protected TreeMsg() {}

    public TreeMsg(TreeMsg  src) {
	m_treeState = src.m_treeState;

	m_arrLfParentDir = new double [2];
	m_arrLfPerpDir = new double [2];
	m_arrLfLoc = new double [2];
	
	m_arrLfParentDir[0] = src.m_arrLfParentDir[0];
	m_arrLfParentDir[1] = src.m_arrLfParentDir[1];
	m_arrLfPerpDir[0] = src.m_arrLfPerpDir[0];
	m_arrLfPerpDir[1] = src.m_arrLfPerpDir[1];
	m_arrLfLoc[0] = src.m_arrLfLoc[0];
	m_arrLfLoc[1] = src.m_arrLfLoc[1];

    }

    public TreeMsg makeCopy() {
    	return new TreeMsg(this);
    }
    
    public TreeMsg(TreeAgentState state,
		   double [] arrLfPositions,
		   int nIdxMyId) {
	m_treeState = state;

	m_arrLfParentDir = new double [2];
	m_arrLfPerpDir = new double [2];
	m_arrLfLoc = new double [2];


	
	m_arrLfLoc[0] = arrLfPositions[2*nIdxMyId];
	m_arrLfLoc[1] = arrLfPositions[2*nIdxMyId+1];
	int nParentId = state.getParentId();
	if(nParentId < 0 || nParentId == nIdxMyId) {
	    m_arrLfParentDir[0] = 1.0;
	    m_arrLfParentDir[1] = 0.0;
	    m_arrLfPerpDir[0] = 0.0;
	    m_arrLfPerpDir[1] = 1.0;
	} else {
	    m_arrLfParentDir[0] = arrLfPositions[2*nParentId] 
		- m_arrLfLoc[0]; 
	    m_arrLfParentDir[1] = arrLfPositions[2*nParentId+1] 
		- m_arrLfLoc[1];
	    double lfScale = m_arrLfParentDir[0]*m_arrLfParentDir[0];
	    lfScale += m_arrLfParentDir[1]*m_arrLfParentDir[1];
	    if(lfScale > 0.0) {
		lfScale = 1.0/Math.sqrt(lfScale);
	    }
	    m_arrLfParentDir[0] *= lfScale;
	    m_arrLfParentDir[1] *= lfScale;
	    m_arrLfPerpDir[0] = -m_arrLfParentDir[1];
	    m_arrLfPerpDir[1] = m_arrLfParentDir[0];
	}
    }


    /**
     * Gets logic var component of message
     */
    public TreeAgentState getState() { return m_treeState; }

    /**
     * Gets one basis vector for parent frame
     */
    public double [] getParentDir() { return m_arrLfParentDir; }

    /**
     * Gets one basis vector for parent frame
     */
    public double [] getPerpDir() { return m_arrLfPerpDir; }
    
    /**
     * Gets origin for parent frame
     */
    public double [] getLoc() { return m_arrLfLoc; }

    /**
     * @param arrLfUv array containing uv coordinates
     * @param nOff offset of uv coordinates
     * @return uv coordinates transformed to world coordinates
     */
    public double [] uvToWorld(double [] arrLfUv, int nOff) {
	double [] arrLfResult = new double[2];
	arrLfResult[0] = m_arrLfLoc[0];
	arrLfResult[1] = m_arrLfLoc[1];

	double u = arrLfUv[nOff]; double v = arrLfUv[nOff+1];

	arrLfResult[0] += u*m_arrLfParentDir[0] +
	    v*m_arrLfPerpDir[0];
	arrLfResult[1] += u*m_arrLfParentDir[1] +
	    v*m_arrLfPerpDir[1];

	return arrLfResult;
    }

    /**
     * @param arrLfWorld array containing world coordinates
     * @param nOff offset of world coordinates
     * @return world coordinates transformed to uv coordinates
     */
    public double [] worldToUv(double [] arrLfWorld, int nOff)
    {
	double [] arrLfResult = new double[2];
	double lfTmpX = arrLfWorld[nOff]-m_arrLfLoc[0];
	double lfTmpY = arrLfWorld[nOff+1]-m_arrLfLoc[1];

	arrLfResult[0] = lfTmpX*m_arrLfParentDir[0] +
	    lfTmpY*m_arrLfParentDir[1];
	arrLfResult[1] = lfTmpX*m_arrLfPerpDir[0] +
	    lfTmpY*m_arrLfPerpDir[1];

	return arrLfResult;
    }

    public double [] getPhase1Targ(double lfD1)
    {
	double [] arrLfOut = new double[2];
	
	// code goes here
	arrLfOut[0] = lfD1;
	arrLfOut[1] = 0.0;

	arrLfOut = uvToWorld(arrLfOut, 0); // 
	
	return arrLfOut;
    }


    public double [] getPhase2Targ(double [] arrLfWorld, int nOff,
				  double lfSpeedTimesTcom)
    {
	double [] arrLfOut = worldToUv(arrLfWorld, nOff);

	double lfDist = 
	    Math.sqrt(arrLfOut[0]*arrLfOut[0] +
		      arrLfOut[1]*arrLfOut[1]);

	double lfSinTheta, lfCosTheta;
	
	if(lfDist > lfSpeedTimesTcom) {
	    double lfTheta = lfSpeedTimesTcom/lfDist;
	    lfSinTheta = Math.sin(lfTheta);
	    lfCosTheta = Math.cos(lfTheta);
	}
	else {
	    lfSinTheta = 0.5*Math.sqrt(3.0);
	    lfCosTheta = 0.5;
	}

	double [] arrLfTmp = new double[2];



	arrLfTmp[0] = arrLfOut[0]*lfCosTheta -
	    arrLfOut[1]*lfSinTheta;
	arrLfTmp[1] = arrLfOut[0]*lfSinTheta +
	    arrLfOut[1]*lfCosTheta;


	arrLfOut = uvToWorld(arrLfTmp, 0);
	
	return arrLfOut;
    }
}