/****************************************************************************/
/* Copyright 1995 to 1998 MBARI                                             */
/****************************************************************************/
/* Summary  : Vehicle Heartbeat Generator for vxWorks                       */
/* Filename : Heartbeat.cc                                                  */
/* Author   : Andrew Pearce                                                 */
/* Project  : Tiburon                                                       */
/* Version  : Version 1.0                                                   */
/* Created  : 05/07/98                                                      */
/* Modified : 05/07/98                                                      */
/* Archived :                                                               */
/****************************************************************************/
/* Modification History:                                                    */
/* $Header:
 * $Log:
 */
/****************************************************************************/

#include <vxWorks.h>                /* vxWorks system declarations          */
#include <semLib.h>                 /* vxWorks semaphore functions          */
#include <taskLib.h>                /* vxWorks task control functions       */
#include <stdioLib.h>               /* vxWorks standard I/O functions       */

#include "mbariTypes.h"             /* MBARI style guide type declarations  */
#include "mbariConst.h"             /* Miscellaneous constants              */

#include "datamgr.h"                /* Data Manager declarations            */
#include "dm_errno.h"               /* Data Manager error declarations      */

#include "dmUtils.h"

#include <DataManager.h>        /* C++ Data Manager Classes                  */
#include <DmGroup.h>            /* C++ Data Manager Group Classes            */

#include "Heartbeat.h"

extern "C"
{
  extern Int32 sysClkRateGet(void);
};

Heartbeat::Heartbeat(const char *heartbeatName, const char *aliveName,
                     Period period)
{
    heartbeat  = 0;
    beatPeriod = period;
    delaySem   = SEM_NULL;
    monitorTaskId = NULL;

    heartbeatDm = new DmNat32Object(heartbeatName);
    aliveDm     = new DmBooleanObject(aliveName);

    if (heartbeatDm->startProvide(period, ExclusiveProvider) == SUCCESS)
      monitorTaskId = taskSpawn("hbMonitor",
		         HEARTBEAT_MONITOR_PRIORITY, 0,
                         HEARTBEAT_MONITOR_STACKSIZE,
                         (FUNCPTR )&Heartbeat::MonitorTask,
                         (int)this, 0, 0, 0, 0, 0, 0, 0, 0, 0);
}

Heartbeat::~Heartbeat()
{
  heartbeatDm->stopProvide();

  if (monitorTaskId != NULL)
    taskDelete(monitorTaskId);
}


STATUS Heartbeat::beat()
{
  DM_Time heartbeatTime;

  heartbeat++;                      /* Increment heartbeat                   */
                                    /* Get time & date from clock            */
  gettimeofday(&heartbeatTime, (struct timezone *)NULL);
  return heartbeatDm->write(heartbeat, &heartbeatTime);
}

STATUS Heartbeat::delay()
{
  if (delaySem == SEM_NULL)
    {                               /* Create period semaphore               */
      if ((delaySem = semBCreate(SEM_Q_FIFO, SEM_EMPTY)) == NULL )
	{
	  printf("Error creating semaphore\n");
	  return ERROR;
	} /* if */
    }
                                    /* Wait heartbeat time                   */
  semTake(delaySem, (beatPeriod * sysClkRateGet()) / USECS_PER_SEC);
  return OK;
} 

STATUS Heartbeat::MonitorTask( Heartbeat *hbObj )
{
    SEM_ID  wakeupSem;
    DM_Time heartbeatTime, lastTime;/* Heartbeat Update Time                */

    Nat32   heartbeat, lastHeartbeat;/* Heartbeat value                     */
    MBool   isAlive,   lastAlive;    /* Alive Flag                          */

                                    /* Create wakeup semaphore              */
    if ((wakeupSem = semBCreate(SEM_Q_FIFO, SEM_EMPTY)) == NULL )
    {
        printf("Heartbeat Monitor Task: Error creating semaphore\n");
        return(ERROR);
    } /* if */

    hbObj->aliveDm->startProvide(DM_STATIC, ExclusiveProvider);
    hbObj->heartbeatDm->startConsume(hbObj->beatPeriod, wakeupSem);

    if (hbObj->heartbeatDm->read(&heartbeat, &heartbeatTime) != SUCCESS)
    {
        printf("Heartbeat Monitor: Can't read Heartbeat\n");
        return (ERROR);
    } /* if */

    lastAlive = TRUE;              /* Assume its alive so we write the Data */
    isAlive   = TRUE;

    hbObj->aliveDm->write(isAlive, &heartbeatTime);

    FOREVER
    {
        lastHeartbeat    = heartbeat;
        lastTime.tv_sec  = heartbeatTime.tv_sec;
        lastTime.tv_usec = heartbeatTime.tv_usec;
        lastAlive        = isAlive;

                                   /* Wait heartbeat time                    */
        semTake(wakeupSem, (hbObj->beatPeriod * sysClkRateGet())
                          / USECS_PER_SEC + sysClkRateGet());

	if (hbObj->heartbeatDm->read(&heartbeat, &heartbeatTime) == SUCCESS)
        {                          /* Decide if task or system is alive      */
            isAlive = ( (heartbeat != lastHeartbeat) &&
                    ((lastTime.tv_sec  !=  heartbeatTime.tv_sec) ||
                     (lastTime.tv_usec !=  heartbeatTime.tv_usec)) );

                                   /* Change in Alive State so write item    */
            if (isAlive != lastAlive)
	      {                    /* Get time & date from clock             */
		gettimeofday(&heartbeatTime, (struct timezone *)NULL);
                                   /* Write item to Data Manager             */
		hbObj->aliveDm->write(isAlive, &heartbeatTime);
	      } /* if */
        } /* if */

        else
            printf("Heartbeat Monitor: Can't read Heartbeat\n");

    } /* FOREVER */
}

#ifdef COMPILE_TEST_CODE


hbTest( Void )
{
  Heartbeat hbObj("HB.TEST.HEARTBEAT", "HB.TEST.ALIVE", USECS_PER_SEC);

  FOREVER
    {
      hbObj.delay();
      hbObj.beat();
    }
}


hbCheck( Void )
{
  DmBooleanObject *aliveDm;
  SEM_ID  wakeupSem;

  MBool alive;
                                    /* Create wakeup semaphore              */
  if ((wakeupSem = semBCreate(SEM_Q_FIFO, SEM_EMPTY)) == NULL )
    {
      printf("hbCheck Task: Error creating semaphore\n");
      return(ERROR);
    } /* if */

  aliveDm = new DmBooleanObject("HB.TEST.ALIVE");
  aliveDm->startConsume(DM_STATIC, wakeupSem);

  FOREVER
    {
      semTake(wakeupSem, WAIT_FOREVER);
      aliveDm->read(&alive, (DM_Time *) NULL);
 
      if (alive)
	printf("Alive\n");
      else
	printf("Bring out yer dead\n");
    }
}


#endif