/****************************************************************************/
/* Copyright 1995 to 1996 MBARI                                             */
/****************************************************************************/
/* Summary  : Tiburon Status Monitor for vxWorks                            */
/* Filename : tiburonStatus.c                                               */
/* Author   : Andrew Pearce                                                 */
/* Project  : Tiburon                                                       */
/* Version  : Version 1.0                                                   */
/* Created  : 10/12/94                                                      */
/* Modified : 04/04/96                                                      */
/* Archived :                                                               */
/****************************************************************************/
/* Modification History:                                                    */
/* $Header:
 * $Log:
 */
/****************************************************************************/

#include <vxWorks.h>                /* vxWorks system declarations          */
#include <semLib.h>                 /* vxWorks semaphore functions          */
#include <stdioLib.h>               /* vxWorks standard I/O functions       */
#include <msgQLib.h>                /* vxWorks Message Queue Library        */

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

#include "rovPriority.h"            /* MBARI Rov Application Priorities     */
#include "usrTime.h"                /* MBARI time and date functions        */

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

#include "tiburon.h"                /* Tiburon definitions                  */
#include "pduMicro.h"               /* PDU Micro Definitions                */
#include "powerAlloc.h"             /* Tiburon Power Draws                  */

#define STATUS_MONITOR_RATE TIBURON_HEARTBEAT_RATE

/****************************************************************************/
/* Function    : tiburonStatusTask                                          */
/* Purpose     : Generates Vehicle Heartbeat                                */
/* Inputs      : NONE                                                       */
/* Outputs     : NONE, but DM Item is modified                              */
/****************************************************************************/
   STATUS
tiburonStatusTask( Void )
{
    SEM_ID  wakeupSem;              /* Used wake up this task periodically  */
    DM_Item heartBeatDm;            /* Vehicle Heartbeat Data Manager Item  */
    DM_Item tiburonPowerOnDm;       /* Vehicle Power On Data Manager Item   */
    DM_Item tiburonAliveDm;         /* Vehicle Alive Data Manager Item      */
    DM_Item pduPowerDm;             /* PDU Power Data Manager Item          */
    DM_Time heartbeatTime, lastTime;/* Vehicle Heartbeat Update Time        */

    Nat32  heartbeat, lastHeartbeat;/* Vehicle Heartbeat                    */
    MBool  tiburonAlive, lastAlive; /* Vehicle Alive Flag                   */
    MBool  tiburonOn, lastTiburonOn;/* Vehicle On Flag                      */
    Flt32  pduPower;                /* Power draw by cable & tiburon        */

                                    /* Set this task priority               */
    taskPrioritySet(taskIdSelf(), TIBURON_STATUS_PRIORITY);

                                    /* Create wakeup semaphore              */
    if ((wakeupSem = semBCreate(SEM_Q_FIFO, SEM_EMPTY)) == NULL )
    {
        logMsg("Vehicle Status Task: Error creating semaphore\n");
        return;
    } /* if */

    tiburonAliveDm =
        initMicroDmItem(TIBURON_DM_PREFIX, TIBURON_ALIVE_DM, DM_MBOOL, 1);

    tiburonPowerOnDm =
        initMicroDmItem(TIBURON_DM_PREFIX, TIBURON_POWER_ON_DM, DM_MBOOL, 1);

    heartBeatDm =
        initMicroDmItem(TIBURON_DM_PREFIX, TIBURON_HEARTBEAT_DM, DM_NAT32, 1);

    pduPowerDm =
        initMicroDmItem(PDU_MICRO_DM_PREFIX, WATTAGE_DM, DM_FLT32, 1);

    dm_start_provider(tiburonPowerOnDm, DM_STATIC);
    dm_start_provider(tiburonAliveDm,   DM_STATIC);
    dm_start_consumer(heartBeatDm, TIBURON_HEARTBEAT_RATE, wakeupSem);
    dm_start_consumer(pduPowerDm,  STATUS_MONITOR_RATE,    SEM_NULL);

    if (dm_read( heartBeatDm, (char *) &heartbeat, sizeof(heartbeat),
        &heartbeatTime) != SUCCESS)
    {
        logMsg("tiburonStatus: Can't read Tiburon Heartbeat\n");
        return (ERROR);
    } /* if */

    lastAlive = FALSE;             /* Assume its dead so we write the Data   */
    lastTiburonOn = FALSE;         /* Manager Item if its not                */
    tiburonAlive = FALSE;

    writeBooleanDmItem(tiburonAliveDm, tiburonAlive);

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

                                   /* Wait heartbeat time + 1 second         */
        semTake(wakeupSem, sysClkRateGet() *
                ((TIBURON_HEARTBEAT_RATE / USECS_PER_SEC) + 2));

        if (dm_read( heartBeatDm, (char *) &heartbeat, sizeof(heartbeat),
           &heartbeatTime) == SUCCESS)
        {                          /* Decide if tiburon is alive             */
            tiburonAlive = ( (heartbeat != lastHeartbeat) &&
                ((lastTime.tv_sec  !=  heartbeatTime.tv_sec) ||
                 (lastTime.tv_usec !=  heartbeatTime.tv_usec)) );

                                   /* Change in Alive State so write item    */
            if (tiburonAlive != lastAlive)
                writeBooleanDmItem(tiburonAliveDm, tiburonAlive);
        } /* if */
        else
            logMsg("tiburonStatus: Can't read Tiburon Heartbeat\n");

        if (dm_read( pduPowerDm, (char *) &pduPower, sizeof(pduPower),
           (DM_Time *) NULL) == SUCCESS)
        {
            lastTiburonOn = tiburonOn;

            tiburonOn = (pduPower >= TIBURON_CABLE_POWER);

                                   /* Vehicle Power On State changed         */
            if (tiburonOn != lastTiburonOn)
                writeBooleanDmItem(tiburonPowerOnDm, tiburonOn);

        } /* if */

    } /* FOREVER */
} /* tiburonStatusTask() */