/**
 * <p>
 * Control function implementing "rotate about point"
 * </p>
 * @author     Michael Schuresko
 * @version    %I%, %G%
 * @since      1.0
 */
public class ControlFuncRotateAbout implements IControlFunc
{

    double m_velMag;
    double [] m_arrLfCenter;
    double m_lfEpsilon;

    public ControlFuncRotateAbout() 
    {
	m_velMag = 0.0; m_arrLfCenter = null;
	m_lfEpsilon = 0.00000001;
    }

    public ControlFuncRotateAbout(ControlFuncRotateAbout src)
    {
	setRotCenter(src.m_arrLfCenter);
	setVel(src.m_velMag);
	m_lfEpsilon = src.m_lfEpsilon;
    }

    public ControlFuncRotateAbout(double [] arrPtTowards, double lfVel)
    {
	setRotCenter(arrPtTowards); setVel(lfVel);
	m_lfEpsilon = 0.00000001;
    }


    /**
     * sets "epsilon" (floating point values "epsilon" apart
     * are considered to be equal)
     * Meaningful to numerical simulation, but meaningless
     * to underlying system being modelled.
     */
    public void setEpsilon( double lfNewEpsilon)
    {
	m_lfEpsilon = lfNewEpsilon;
    }

    /**
     * gets "epsilon" (floating point values "epsilon" apart
     * are considered to be equal)
     * Meaningful to numerical simulation, but meaningless
     * to underlying system being modelled.
     */
    public double getEpsilon() { return m_lfEpsilon; }

    /**
     * sets a new point to move towards, possibly re-assigning
     * control function dimension (some care must be taken by programmer
     * here)
     */
    public void setRotCenter(double [] arrPtTowards)
    {
	if(m_arrLfCenter == null || 
	   m_arrLfCenter.length != arrPtTowards.length) 
	    {
		m_arrLfCenter = new double[arrPtTowards.length];
	    }
	for(int i = 0; i < arrPtTowards.length; ++i)
	    {
		m_arrLfCenter[i] = arrPtTowards[i];
	    }
    }

    /**
     * Use of getRotCenter is highly discouraged -- intend to deprecate
     */
    public double [] getRotCenter()
    {
	return m_arrLfCenter;
    }

    public void setVel(double lfVel) { m_velMag = lfVel; }

    /**
     * clones this object (similar to "clone" supported by IClonable 
     * interface).
     */
    public IControlFunc makeCopy()
    {
	return new ControlFuncRotateAbout(this);
    }

    /**
     * <p>
     * Updates arrLfDerivDst with the instantaneous derivatives
     * of the continuous-time component of the agent's state 
     * given its current state and sensor readings
     * </p>
     * @param lfCurrTime current time (for time-varying systems)
     * @param arrLfStateSrc vector of global state of the system
     * @param sensors sensors 
     * @param env environment
     * @param logicVars discrete components of agent internal state
     * @param arrLfDerivDst Destination for storing derivative values.
     * @param nIdxOff Offset index into the global state vector 
     * corresponding to the beginning of the parameters for this agent.
     * @since 1.0
     */
    public void getDerivs(double lfCurrTime, 
			  double arrLfStateSrc[], 
			  ISensor sensors,
			  IEnvironment env,
			  ILogicVarBundle logicVars,
			  double arrLfDerivDst[],
			  int nIdxOff)
    {
	double lfMagScale = 0.0;
	double lfSign = -1.0;
	for(int i = 0; i < m_arrLfCenter.length; ++i)
	    {
		int iNext = (i+1)%m_arrLfCenter.length;
		
		arrLfDerivDst[nIdxOff+i] = 
		    lfSign*(
			    m_arrLfCenter[iNext] - 
			    arrLfStateSrc[nIdxOff+iNext]);
		lfMagScale +=
		    arrLfDerivDst[nIdxOff+iNext] *
		    arrLfDerivDst[nIdxOff+iNext];

		lfSign *= -1.0;
	    }
	if(lfMagScale < m_lfEpsilon*m_lfEpsilon)
	    {
		return;
	    }
	lfMagScale = m_velMag/Math.sqrt(lfMagScale);

	for(int i = 0; i < m_arrLfCenter.length; ++i)
	    {
		arrLfDerivDst[nIdxOff+i] *= lfMagScale;
	    }
    }
   
    /**
     * returns dimensionality of the state vector corresponding to one agent
     * @return dimensionality of the state vector corresponding to one agent
     */
    public int getNumStateVars()
    {
	if(m_arrLfCenter == null) { return 0; }
	return m_arrLfCenter.length;
    }

    
    public boolean directionConstantQ(double lfCurrTime, 
			  double arrLfStateSrc[], 
			  ISensor sensors,
			  IEnvironment env,
			  ILogicVarBundle logicVars,
			  int nIdxOffsetState) {
  	return false;
  }
 
    
}