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

    double m_velMag;
    double m_lfThetaFinal;
    double m_lfCurrTheta;

    double m_lfIniTheta;
    double m_lfSinIniTheta;
    double m_lfCosIniTheta;

    boolean m_bCurrThetaInit;
    double [] m_arrLfCenter;
    double m_lfEpsilon;

    public ControlFuncRotateAboutByTheta() 
    {
	m_velMag = 0.0; m_arrLfCenter = null;
	m_lfEpsilon = 0.00000001;
	m_lfThetaFinal = Math.PI;
	m_bCurrThetaInit = false;
    }

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

	m_lfCurrTheta = src.m_lfCurrTheta;
	m_bCurrThetaInit = src.m_bCurrThetaInit;
	m_lfThetaFinal = src.m_lfThetaFinal;

	m_lfIniTheta = src.m_lfIniTheta;
	m_lfSinIniTheta = src.m_lfSinIniTheta;
	m_lfCosIniTheta = src.m_lfCosIniTheta;
    }

    public ControlFuncRotateAboutByTheta(double [] arrPtTowards, double lfVel,
					 double lfThetaMax)
    {
	setRotCenter(arrPtTowards); setVel(lfVel);
	m_lfEpsilon = 0.00000001;
	m_lfThetaFinal = lfThetaMax;
	m_bCurrThetaInit = false;
    }


    /**
     * 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 ControlFuncRotateAboutByTheta(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 lfXDiff =  arrLfStateSrc[nIdxOff] -
	    m_arrLfCenter[0];
	double lfYDiff =  arrLfStateSrc[nIdxOff+1] -
	    m_arrLfCenter[1];

	double lfMagScale = lfXDiff*lfXDiff + lfYDiff*lfYDiff;

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

	lfXDiff *= lfMagScale; lfYDiff *= lfMagScale;
	
	if(!m_bCurrThetaInit) {
	    m_bCurrThetaInit = true;
	    m_lfIniTheta = Math.atan2(lfYDiff, lfXDiff);
				     
	    m_lfSinIniTheta = Math.sin(m_lfIniTheta);
	    m_lfCosIniTheta = Math.cos(m_lfIniTheta);
	}
	
	// this strange line of code implicitly rotates
	// the vector from which currTheta originates back by
	// m_lfIniTheta (note the flipped signs)
	// this way I don't have to worry about sign changes when comparing
	// angles that wrap around 2pi.
	m_lfCurrTheta = 
	    Math.atan2(lfYDiff*m_lfCosIniTheta - lfXDiff*m_lfSinIniTheta,
		       lfXDiff*m_lfCosIniTheta + lfYDiff*m_lfSinIniTheta);
	// 
	
	
	arrLfDerivDst[nIdxOff] = -m_velMag*lfYDiff;
	arrLfDerivDst[nIdxOff+1] = m_velMag*lfXDiff;

	if(m_lfCurrTheta > m_lfThetaFinal) {
	    arrLfDerivDst[nIdxOff] = arrLfDerivDst[nIdxOff+1] = 0.0;
	}
	   
	
    }
    
    public boolean directionConstantQ(double lfCurrTime, 
			  double arrLfStateSrc[], 
			  ISensor sensors,
			  IEnvironment env,
			  ILogicVarBundle logicVars,
			  int nIdxOffsetState) {
  	return false;
  }
 
   
    /**
     * 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;
    }

    
}