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

    double m_velMag;
    double [] m_arrPtTowards;
    double m_lfEpsilon;

    int m_nFlag;
    
    public ControlFuncMoveTowards() 
    {
	m_velMag = 0.0; m_arrPtTowards = null;
	m_lfEpsilon = 0.00000001;
	m_nFlag = -1;
    }

    public ControlFuncMoveTowards(ControlFuncMoveTowards src)
    {
	setTowards(src.m_arrPtTowards);
	setVel(src.m_velMag);
	m_lfEpsilon = src.m_lfEpsilon;
	m_nFlag = src.m_nFlag;
    }

    public ControlFuncMoveTowards(double [] arrPtTowards, double lfVel)
    {
	setTowards(arrPtTowards); setVel(lfVel);
	m_lfEpsilon = 0.00000001;
	m_nFlag = -1;
    }
    
    public ControlFuncMoveTowards(double [] arrPtTowards, double lfVel, int nFlag)
    {
	setTowards(arrPtTowards); setVel(lfVel);
	m_lfEpsilon = 0.00000001;
	m_nFlag = nFlag;
    }
    
    public ControlFuncMoveTowards(double lfXDst, double lfYDst, double lfVel)
    {
    	m_arrPtTowards = new double[2];
    	m_arrPtTowards[0] = lfXDst; m_arrPtTowards[1] = lfYDst;
    	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 setTowards(double [] arrPtTowards)
    {
	if(m_arrPtTowards == null || 
	   m_arrPtTowards.length != arrPtTowards.length) 
	    {
		m_arrPtTowards = new double[arrPtTowards.length];
	    }
	for(int i = 0; i < arrPtTowards.length; ++i)
	    {
		m_arrPtTowards[i] = arrPtTowards[i];
	    }
    }

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

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

    public double getVel() { return m_velMag; }
    
    /**
     * clones this object (similar to "clone" supported by IClonable 
     * interface).
     */
    public IControlFunc makeCopy()
    {
	return new ControlFuncMoveTowards(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;
	for(int i = 0; i < m_arrPtTowards.length; ++i)
	    {
		arrLfDerivDst[nIdxOff+i] = 
		    m_arrPtTowards[i] - 
		    arrLfStateSrc[nIdxOff+i];
		lfMagScale +=
		    arrLfDerivDst[nIdxOff+i] *
		    arrLfDerivDst[nIdxOff+i];
	    }
	
	if(lfMagScale < m_lfEpsilon*m_lfEpsilon)
	    {
		return;
	    }
	lfMagScale = m_velMag/Math.sqrt(lfMagScale);

	for(int i = 0; i < m_arrPtTowards.length; ++i)
	    {
		arrLfDerivDst[nIdxOff+i] *= lfMagScale;
	    }
    }
    
    public boolean directionConstantQ(double lfCurrTime, 
			  double arrLfStateSrc[], 
			  ISensor sensors,
			  IEnvironment env,
			  ILogicVarBundle logicVars,
			  int nIdxOffsetState) {
  	return true;
  }
 
   
    /**
     * 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_arrPtTowards == null) { return 0; }
	return m_arrPtTowards.length;
    }

    
}