#include <carcontrolImpl.h>
#include "acsexmplCallbacks.h"

SpeedControlThread::SpeedControlThread(
    const ACE_CString& name, 
    CarControlImpl *_car_p, 
    bool suspended,
    const ACS::TimeInterval& responseTime, 
    const ACS::TimeInterval& sleepTime) :
    ACS::Thread(name, suspended, responseTime, sleepTime),
    m_car_p(_car_p)
{
    ACS_TRACE("SpeedControlThread::SpeedControlThread");
}

SpeedControlThread::~SpeedControlThread() 
{
    ACS_TRACE("SpeedControlThread::~SpeedControlThread"); 
}

void SpeedControlThread::runLoop()
{
    ACS_STATIC_TRACE("SpeedControlThread::runLoop");

    unsigned long long timestamp;

    double setSpeed = m_car_p->m_setSpeed_sp->getDevIO()->read(timestamp);
    double actSpeed = m_car_p->m_actSpeed_sp->getDevIO()->read(timestamp);
    double pressure = m_car_p->m_pressure_sp->getDevIO()->read(timestamp);
    double increment = 10 * pressure;
    try
	{
	if(setSpeed > actSpeed)
	    m_car_p->m_actSpeed_sp->getDevIO()->write(actSpeed+increment, timestamp);
	if(setSpeed < actSpeed)
	    m_car_p->m_actSpeed_sp->getDevIO()->write(actSpeed-increment, timestamp);
	}
    catch (...) 
	{
	// Here we have to better handle errors!
	ACS_SHORT_LOG((LM_ERROR,"Error accessing devIO"));
	}
}

/* ----------------------------------------------------------------*/
CarControlImpl::CarControlImpl(const ACE_CString &_name, 
			       maci::ContainerServices *containerServices) :
    CharacteristicComponentImpl(_name, containerServices),
    m_setSpeed_sp(new RWdouble(_name+":setSpeed", getComponent()),this),
    m_actSpeed_sp(new ROdouble(_name+":actSpeed", getComponent()),this),
    m_pressure_sp(new RWdouble(_name+":pressure", getComponent()),this),
    onCallback("on")
{
    ACS_TRACE("::CarControlImpl::CarControlImpl");

    // Initialize control loop thread
    SpeedControlThread *sct = new SpeedControlThread("speedControl",this);
    getContainerServices()->getThreadManager()->add("speedControl",sct);

}

CarControlImpl::~CarControlImpl()
{
    ACS_TRACE("::CarControlImpl::~CarControlImpl");
    ACS_DEBUG_PARAM("::CarControlImpl::~CarControlImpl", "Destroying %s...", name());
}

void
CarControlImpl::execute()
    throw (acsErrTypeLifeCycle::LifeCycleExImpl)
{
    ACS_SHORT_LOG((LM_INFO,"CarControlImpl::execute"));
#if 0    
    /*
     * Get a reference to the Joystick components
     */    
    try 
	{
	ACE_CString joyName(this->name());
	joyName += "/PAD";
	
	// Use ContainerServices to activate the Joystick Component
	ACS_SHORT_LOG((LM_INFO, "Getting component: %s", joyName.c_str() ));
	
	m_pad_p  = getContainerServices()->getComponent<hptPad::Pad>(joyName.c_str());    

	/*
	 * Switches the joystick driver on()
	 */
	ACS_SHORT_LOG((LM_INFO, "Switching on joystick"));

	ACS::CBDescIn desc;
	    
	m_pad_p->on(onCallback.getCb(), desc);

	}
    catch(...)
	{
	// TBD: Exception handling 
	ACS_SHORT_LOG((LM_ERROR, "::CarControlImpl::execute"));
	}
#endif
}

void CarControlImpl::cleanUp()
    throw (acsErrTypeLifeCycle::LifeCycleExImpl)
{
    ACS_TRACE("::CarControlImpl::cleanUp");
#if 0
    // Probably I should switch it off as well here.

    // must release the door component
    if(CORBA::is_nil(m_pad_p.in()) == false)
	{
	ACS_LOG(LM_RUNTIME_CONTEXT, "::CarControlImpl::cleanUp",
		(LM_DEBUG, "Releasing /PAD"));
	ACE_CString joyName(this->name());
	joyName += "/PAD";

	getContainerServices()->releaseComponent(joyName.c_str());

	// be sure to set the reference to nil
	m_pad_p = hptPad::Pad::_nil();
	}
#endif
}

void CarControlImpl::accelerate (CORBA::Double pressure)
	throw (CORBA::SystemException)
{
    ACS_TRACE("::CarControlImpl::accelerate");
    unsigned long long timestamp;

    try
	{
	m_pressure_sp->getDevIO()->write(pressure, timestamp);
	m_setSpeed_sp->getDevIO()->write(pressure * 300, 
					 timestamp);
	m_actSpeed_sp->getDevIO()->write(pressure * 300, 
					 timestamp);
	}
    catch (...) 
	{
	// Here we have to better handle errors!
	ACS_SHORT_LOG((LM_ERROR,"Error accessing devIO"));
	}
}

void CarControlImpl::brake (CORBA::Boolean on)
	throw (CORBA::SystemException)
{
    ACS_TRACE("::CarControlImpl::brake");
    unsigned long long timestamp;

    if (on)
    {
    try
	{
	m_setSpeed_sp->getDevIO()->write(0, 
					 timestamp);
	m_actSpeed_sp->getDevIO()->write(0, 
					 timestamp);
	}
    catch (...) 
	{
	// Here we have to better handle errors!
	ACS_SHORT_LOG((LM_ERROR,"Error accessing devIO"));
	}
    }
}

ACS::RWdouble_ptr
CarControlImpl::setSpeed ()
    throw (CORBA::SystemException)
{
    if (m_setSpeed_sp == 0)
	{
	return ACS::RWdouble::_nil();
	}

    ACS::RWdouble_var ref = ACS::RWdouble::_narrow(m_setSpeed_sp->getCORBAReference());
    return ref._retn();
}

ACS::ROdouble_ptr
CarControlImpl::actSpeed ()
    throw (CORBA::SystemException)
{
    if (m_actSpeed_sp == 0)
	{
	return ACS::ROdouble::_nil();
	}

    ACS::ROdouble_var ref = ACS::ROdouble::_narrow(m_actSpeed_sp->getCORBAReference());
    return ref._retn();
}

ACS::RWdouble_ptr
CarControlImpl::pressure ()
    throw (CORBA::SystemException)
{
    if (m_pressure_sp == 0)
	{
	return ACS::RWdouble::_nil();
	}

    ACS::RWdouble_var ref = ACS::RWdouble::_narrow(m_pressure_sp->getCORBAReference());
    return ref._retn();
}


/* --------- [ MACI DLL support functions ] -----------------*/
#include <maciACSComponentDefines.h>
MACI_DLL_SUPPORT_FUNCTIONS(CarControlImpl)
/* ----------------------------------------------------------*/


/*___oOo___*/