/*******************************************************************************
*    ALMA - Atacama Large Millimiter Array
*    (c) Associated Universities Inc., 2002 *
*    (c) European Southern Observatory, 2002
*    Copyright by ESO (in the framework of the ALMA collaboration)
*    and Cosylab 2002, All rights reserved
*
*    This library is free software; you can redistribute it and/or
*    modify it under the terms of the GNU Lesser General Public
*    License as published by the Free Software Foundation; either
*    version 2.1 of the License, or (at your option) any later version.
*
*    This library is distributed in the hope that it will be useful,
*    but WITHOUT ANY WARRANTY; without even the implied warranty of
*    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
*    Lesser General Public License for more details.
*
*    You should have received a copy of the GNU Lesser General Public
*    License along with this library; if not, write to the Free Software
*    Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307  USA
*
*
*
* "@(#) $Id: acsVltMountLoopImpl.cpp,v 1.1 2004/01/28 11:38:34 bjeram Exp $"
*
*/

/*
 * This file contains the code to send a message to VLT Control Model
 * using CCS. 
 * A special container (acsVltContainer) that initialize CCS is needed.
 * There is no need to initialize CCS in the component.
 *
 * In order to make it working you need:
 * 1) to create an environment
 * 2) to make sure that the /etc/service file (of both source and destination
 * hosts includes) contains the relative environments and the port number
 * 3) to make sure that the /etc/passwd file of both source and destination
 * hosts are updated so that the user source user can login to the dst host
 *
 * Points 2 and 3 are needed by SQEMU to estalish the connection between
 * source environment and destination environment.
 *
 * This component is derived from acsVltMountImpl and there you can
 * find the code for the custom DevIO able to read from VLT database.    
 *    
 */
 
#include <acsVltMountLoopImpl.h>
#include "ccs.h"
#include "err.h"
#include "msg.h"
#include "db.h"


#include "eccs.h"
#include "eccsMsg.h"

// Control loop time interval [sec]
const static int CTRL_INTERVAL  = 1;

/* ----------------------------------------------------------------*/
MountLoopImpl::MountLoopImpl(PortableServer::POA_ptr poa, const ACE_CString &_name) :
    CharacteristicComponentImpl(poa, _name),
    MountImpl(poa, _name)
{
    // ACS_TRACE is used for debugging purposes
    ACS_TRACE("::MountLoopImpl::MountLoopImpl");


    // Initialize control loop thread
    getComponent()->getThreadManager()->create("positionControl", 
					       (void *)	MountLoopImpl::positionControl, 
					       static_cast<void *>(this));
    ACS_SHORT_LOG((LM_INFO,"positionControl thread spawned.")); 
}

/* ----------------------------------------------------------------*/
MountLoopImpl::~MountLoopImpl()
{
    // ACS_TRACE is used for debugging purposes
    ACS_TRACE("::MountLoopImpl::~MountLoopImpl");
}

//
// Position control loop
// When the commanded position changes the actual position is moved to this
// new position.
//
void 
MountLoopImpl::positionControl (void *param_p) 
{
    ccsCOMPL_STAT    locstat;
    
    if (param_p == 0) 
	{
	return;
	}
    
    
    ACS::Completion_var completion;
    
    //
    // TODO: Error/Exception handling
    // 
    
    BACIThreadParameter *baciParameter_p = static_cast<BACIThreadParameter *>(param_p);
    BACIThread *myself_p = baciParameter_p->getBACIThread();
    
    // The thread worker is a static method.
    // The instance comes with the param_p structure
    MountLoopImpl *mount_p = const_cast<MountLoopImpl *>(static_cast<const MountLoopImpl *>(baciParameter_p->getParameter()));

    ACS_SHORT_LOG((LM_INFO, "Starting Position control loop."));

    if (BACIThread::InitThread != 0) 
	{
	BACIThread::InitThread("positionControl");
	}

    int errcode;
    unsigned long long timestamp;

    /* Message sending variables */
    msgMESSAGE cmd;
    static msgMESSAGE reply;


    cmd.Command("PRSALAZ");
    cmd.Destproc("tifCA");
    
    // cmd.Destenv("wt0tcs");
    cmd.Destenv("walma");

    int waiting = ccsFALSE;

    // Control loop
    while(myself_p->check() == true)
        {
	if(myself_p->isSuspended() == false)
	    {           
            double cmdAzValue = mount_p->m_cmdAz_p->getDevIO()->read(errcode, timestamp);
            double cmdElValue = mount_p->m_cmdEl_p->getDevIO()->read(errcode, timestamp);
 
            double actAzValue = mount_p->m_actAz_p->getDevIO()->read(errcode, timestamp);
            double actElValue = mount_p->m_actEl_p->getDevIO()->read(errcode, timestamp);

            if ( (cmdAzValue != actAzValue) ||
		 (cmdElValue != actElValue)    )
		{

	        // Simulated control
                ACS_SHORT_LOG((LM_INFO, "Moving from (%F,%f) to (%f,%f)...", 
			       actAzValue, actElValue, cmdAzValue, cmdElValue));
		mount_p->m_actAz_p->getDevIO()->write(cmdAzValue, errcode, timestamp);
		mount_p->m_actEl_p->getDevIO()->write(cmdElValue, errcode, timestamp);

		char str[256];
		sprintf(str, "%7.2f, %7.2f", cmdElValue, cmdAzValue);
		cmd.Buffer(str);

                // Sending command
		locstat = cmd.SendCommand();
		if( locstat == FAILURE )
		    {
		    ACS_SHORT_LOG((LM_INFO, "--> Error in SendCommand"));
		    errDisplay();
		    }
		else
		    {
		    waiting = ccsTRUE;
		    }
		}

	    if (waiting == ccsTRUE)
		{
		locstat = reply.Receive(msgNO_WAIT);
		if (locstat == SUCCESS)
		    {
		    ACS_SHORT_LOG((LM_INFO, "Received reply to command: %s, type: %d, buffer: %s",
				   reply.Command(), reply.Type(), reply.Buffer()));
		    waiting = ccsFALSE;
		    }
	      

		    // Receive the reply
		    ACS_SHORT_LOG((LM_INFO, "waiting for reply..."));
		}
	    myself_p->sleep();
	    }
	ACE_OS::sleep(CTRL_INTERVAL);
	}
    
    if (BACIThread::DoneThread != 0) 
	{
	BACIThread::DoneThread();
	}
    delete baciParameter_p;
    myself_p->setStopped();

 
}

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


/*___oOo___*/