//
//  Copyright © 2004, RESON Inc. All Rights Reserved.
//
//  No part of this file may be reproduced or transmitted in any form or by
//  any means, electronic or mechanical, including photocopy, recording, or
//  information storage or retrieval system, without permission in writing
//  from RESON Inc.
//
//  Filename:   VehicleData.cpp
//
//  Project:    6046
//
//  Author(s):  W. Arcus
//
//  Purpose:    
//
//  Notes:      
//

#include "StdAfx.h"
#include "BlueFinVC.h"
#include "VehicleData.h"

#include "..\..\..\Utils\NetUtils\7kProtocol.h"
#include "..\..\..\Utils\NetUtils\SystemTime.h"

#include <algorithm>

#ifdef _DEBUG
#undef THIS_FILE
static char THIS_FILE[]=__FILE__;
#define new DEBUG_NEW
#endif

namespace                                                                                               // Begin anonymous namespace.
{
    // Constants.

    const unsigned long     ulMaxFrameSize_c                    = max(  sizeof( tagBLUEFINNAVIGATIONMESSAGE::Size() ),
                                                                        sizeof( tagBLUEFINENVIRONMENTMESSAGE::Size() ) );

    const unsigned long     ulNavFrameRate_c                    = 25UL;
    const unsigned long     ulEnvFrameRate_c                    = 5UL;

    const unsigned long     ulMaxFrames_c                       = max( ulNavFrameRate_c, ulEnvFrameRate_c );

    const unsigned long     ulMax7kRecordSize_c                 = tagRECORDHEADER7K::Size() +
                                                                      tagBLUEFINRECORDTYPEHEADER::Size() +
                                                                          ulMaxFrameSize_c * ulMaxFrames_c +
                                                                              Checksum7kSize_m();

    const unsigned short    unDefaultPort_c                     = 3100U;

    const char * const      pszSensorName_c                     = "BlueFin VC";

    const unsigned long     ulDataTimeoutPeriodMilliseconds_c   = 1000UL;                               // For now, time out period for data detection is 1s.

}                                                                                                       // End anonymous namespace.

//////////////////////////////////////////////////////////////////////
// CVehicleData class implementation.

using std::auto_ptr;
using namespace NBlueFin7kMessages;

auto_ptr<CVehicleData::CVehicleInterface>   CVehicleData::m_pVehicleIF( NULL );

CVehicleData::CVehicleData( const int &riSensorIndex )
             :CSensor( riSensorIndex )
{
    m_Address = _T( "127.0.0.1" );
    m_unPort  = unDefaultPort_c;
}

CVehicleData::~CVehicleData( void )
{
    __TRY
    {
        // Only after the last sensor object is being destroyed do we destroy the vehicle sensor interface. Note: we
        // do not perform this in Shutdown() incase the client doesn't call it.

        Disconnect();

        m_Address.Empty();
    }
    __FINALLY
    {
        m_unPort = 0;
    }
    __ENDFINALLY
}

bool CVehicleData::Configure(   const CString              &rAddress,
                                const unsigned short       &runPort )
{
    m_Address = rAddress;
    m_unPort  = runPort;

    return ( ( ! m_Address.IsEmpty() ) && ( m_unPort > 0UL ) );
}

bool CVehicleData::Description( unsigned long &rulDeviceId, int &riSensorType, char *pszName ) const
{
    rulDeviceId     = deviceIdBlueFinVC;
    riSensorType    = sensorTypePOS;

    ASSERT( pszName != NULL );

    if ( pszName != NULL )
    {
        strncpy( pszName, pszSensorName_c, processInfoNameLength );

        return true;
    }

    return false;
}

void CVehicleData::UpdateSensorHealth( void )
{
    // Invoked by the watchdog thread to maintain timeout/health status.

    try
    {
        ASSERT( m_pVehicleIF.get() != NULL );

        if ( m_pVehicleIF.get() == NULL )
        {
            ASSERT( false );
            TRACE( _T( "CVehicleData::UpdateSensorHealth()" ) );
        }
        else if ( m_HealthStatus.UpdateStatus( ! m_pVehicleIF->IsTimedOut() ) )
        {
            CString   sDescription;
            const int iSensorIndex = SensorIndex();
            bool      bResponding  = m_HealthStatus.IsResponsive();

            // If the sensor is responsive then retract the alarm otherwise raise it.

            if ( bResponding )
            {
                sDescription.Format( "%d, Sensor responding.", iSensorIndex );
            }
            else
            {
                sDescription.Format( "%d, Sensor not responding.", iSensorIndex );
            }

            const char *pszMessage = static_cast<const char *>( static_cast<LPCTSTR>( sDescription ) );

            if ( ! ReportAlarm( ( ! bResponding ), alarmIdSensorNotResponding, const_cast<char *>( pszMessage ) ) )
            {
                TRACE( _T( "CVehicleData::UpdateSensorHealth(), ReportAlarm() failed, %s.\n" ), pszMessage );
            }
        }
    }
    catch ( ... )
    {
        TRACE( _T( "CVehicleData::UpdateSensorHealth(), Unspecified exception caught.\n" ) );
    }
}

//////////////////////////////////////////////////////////////////////
// Overrides from CSensor pure base class; constitue the generic sensor interface methods.

bool CVehicleData::Startup  (   const int           &riSensorIndex,
                                const DWORD         &rdwThreadId,
                                const unsigned int  &ruiDataReadyMessageId,
                                const unsigned int  &ruiReplyReadyMessageId,
                                const int           &riActivateOnStartup )
{
    bool bSuccess = false;      // Assume failure for now.

    try
    {
        if ( ! CSensor::Startup( riSensorIndex, rdwThreadId, ruiDataReadyMessageId, ruiReplyReadyMessageId, riActivateOnStartup ) )
        {
            ThrowMessage_m( "CSensor::Startup() failed" );
        }

        // Add a reference count and start the UDP interface if it's the first sensor.

        AddRef();

        if ( RefCount() == 1 )
        {
            ASSERT( m_pVehicleIF.get() == NULL );

            if ( m_pVehicleIF.get() == NULL )
            {
                m_pVehicleIF = auto_ptr<CVehicleData::CVehicleInterface>( new CVehicleInterface( m_unPort ) );

                if ( m_pVehicleIF.get() == NULL )
                {
                    ThrowMessage_m( "Can't construct m_pVehicleIF object" );
                }
            }
        }

        if ( m_pVehicleIF.get() != NULL )
        {
            if ( m_pVehicleIF->Subscribe( this ) )
            {
                bSuccess = true;
            }
            else
            {
                TRACE( _T( "CVehicleData::Startup(), m_pVehicleIF->Subscribe() failed.\n" ) );
            }
        }
    }
    catch ( LPCTSTR lpszMessage )
    {
        bSuccess = false;
        TRACE( _T( "CVehicleData::Startup(), %s.\n" ), lpszMessage );
    }
    catch ( ... )
    {
        bSuccess = false;
        TRACE( _T( "CVehicleData::Startup(), Unspecified exception caught.\n" ) );
    }

    m_bStarted = bSuccess;

    return bSuccess;
}

bool CVehicleData::Shutdown( void )
{
    Disconnect();

    return true;
}

bool CVehicleData::Load(    BYTE                       *pbyData,
                            unsigned long              *pulSize,
                            unsigned long              *pulTimeStamp )
{
    UNREFERENCED_PARAMETER( pbyData );
    UNREFERENCED_PARAMETER( pulSize );
    UNREFERENCED_PARAMETER( pulTimeStamp );

    // This method is obsolete. Data is now loaded via the PLC's sensor data pool directly by each sensor.

    return false;
}

bool CVehicleData::SendCommand( int     iClientIndex,
                                char   *pszCommand )
{
    UNREFERENCED_PARAMETER( iClientIndex );
    UNREFERENCED_PARAMETER( pszCommand );

    // No external commands accepted by this sensor.

    return false;
}

bool CVehicleData::RetrieveStatus(  BYTE                       *pbyData,
                                    unsigned long              *pulSize,
                                    unsigned long              *pulTimeStamp,
                                    int                        *piClientIndex )
{
    UNREFERENCED_PARAMETER( pbyData );

    *pulSize       = 0UL;
    *pulTimeStamp  = 0UL;
    *piClientIndex = -1;

    // Status message isn't relevant to this sensor type, so indicate no data to retrieve.

    return false;
}

bool CVehicleData::IsSensorHealthy( void )
{
    return m_HealthStatus.IsResponsive();
}

bool CVehicleData::Reset( void )
{
    bool bSuccess = false;

    try
    {
        ASSERT( m_pVehicleIF.get() != NULL );

        bSuccess = ( ( m_pVehicleIF.get() != NULL ) && ( m_pVehicleIF->Reset() ) );

        m_HealthStatus.UpdateStatus( bSuccess, true );
    }
    catch ( ... )
    {
        bSuccess = false;
        TRACE( _T( "CVehicleData::Reset(), Unspecified exception caught.\n" ) );
    }

    return bSuccess;
}

bool CVehicleData::RouteMessage(    const BYTE             *pbyMessage,
                                    const unsigned long    &rulSize,
                                    const unsigned long    &rulTimeStamp )
{
    UNREFERENCED_PARAMETER( pbyMessage );
    UNREFERENCED_PARAMETER( rulSize );
    UNREFERENCED_PARAMETER( rulTimeStamp );

    // Routed inbound 7k messages not accepted by this sensor.

    return false;
}

bool CVehicleData::Identify( SENSORINFO *psSensorInfo )
{
    return CSensor::Identify( psSensorInfo );
}

///////////////////////////////////////////////////////////////////////////////
// Internal private helpers.

void CVehicleData::Disconnect( void )
{
    __TRY
    {
        if ( m_pVehicleIF.get() != NULL )
        {
            if ( ! m_pVehicleIF->UnSubscribe( SensorIndex() ) )
            {
                TRACE( _T( "CVehicleData::Shutdown(), m_pVehicleIF->UnSubscribe() failed" ) );
            }
        }
    }
    __FINALLY
    {
        if ( ( ReleaseRef() == 0 ) && ( m_pVehicleIF.get() != NULL ) )
        {
            delete ( m_pVehicleIF.release() );
            m_pVehicleIF = auto_ptr<CVehicleData::CVehicleInterface>( NULL );
        }
    }
    __ENDFINALLY
}

bool CVehicleData::MessageHandler(  const BYTE             *pby7kRecord,
                                    const unsigned long    &rulBytes,
                                    const unsigned long    &rulTimeStamp )
{
    bool bSuccess = false;

    try
    {
        if ( rulBytes == 0UL )
        {
            bSuccess = true;        // Nothing to do.
        }
        else if ( pby7kRecord == NULL )
        {
            bSuccess = false;
            TRACE( _T( "CVehicleData::MessageHandler(), pbyMessage is NULL" ) );
        }
        else
        {
            RECORDHEADER sRecordHeader             = { 0 };

            sRecordHeader.m_iSensorIndex           = SensorIndex();
            sRecordHeader.m_ulRecordCounter        = 0UL;
            sRecordHeader.m_ulRecordSizeInBytes    = rulBytes;
            sRecordHeader.m_ulMillisecondTimestamp = rulTimeStamp;

            bSuccess = m_SensorDataPool.Write( &sRecordHeader, pby7kRecord, rulBytes );
        }
    }
    catch ( ... )
    {
        bSuccess = false;
        TRACE( _T( "CVehicleData::MessageHandler(), Unspecified expected exception caught\n" ) );
    }

    return bSuccess;
}

///////////////////////////////////////////////////////////////////////////////
// CVehicleInterface class implementation -- an internal (private) helper class and friend
// of CVehicleData.

inline
CVehicleData::CVehicleInterface::CVehicleInterface( const unsigned short &runPort )

                                :m_ulMax7kRecordSize                ( ulMax7kRecordSize_c ),
                                 m_ulNavigationFramesPerRecord      ( ulNavFrameRate_c ),
                                 m_ulEnvironmentalFramesPerRecord   ( ulEnvFrameRate_c ),
                                 m_ulDataTimeOut                    ( ulDataTimeoutPeriodMilliseconds_c )
{
    m_bInitialized = false;

    try
    {
        m_unPort = runPort;

        m_CriticalParameters.reserve( criticalParameterMaxParameters );
        m_CriticalParameters.clear();
        m_SubscriberList.clear();

        m_pRecord = auto_ptr<Buffer_t>( new Buffer_t( m_ulMax7kRecordSize ) );
        ASSERT( m_pRecord.get() != NULL );

        m_pSocketServer = auto_ptr<CUDPSocket>( new CUDPSocket( CUDPSocket::modeServer, runPort, false, 0UL, ReceiveHandler, this ) );
        ASSERT( m_pSocketServer.get() != NULL );

        m_NavigationFrameQueue.ConfigureFrame(  m_ulNavigationFramesPerRecord,
                                                    tagBLUEFINNETWORKHEADER::Size() + tagBLUEFINNAVIGATIONMESSAGE::Size(),
                                                        CFrameQueue::frameTypeNavigation );

        m_EnvironmentFrameQueue.ConfigureFrame( m_ulEnvironmentalFramesPerRecord,
                                                    tagBLUEFINNETWORKHEADER::Size() + tagBLUEFINENVIRONMENTMESSAGE::Size(),
                                                        CFrameQueue::frameTypeEnvironment );

        __TRY
        {
            m_FlagsCriticalSection.Enter();
            m_ulTimeStampOfLastUpdate = 0UL;
        }
        __FINALLY
        {
            m_FlagsCriticalSection.Leave();
        }
        __ENDFINALLY

        m_bInitialized = true;
    }
    catch ( ... )
    {
        m_bInitialized = false;
        TRACE( _T( "CVehicleData::CVehicleInterface::CVehicleInterface(), Unspecified exception caught\n" ) );
    }
}

inline
CVehicleData::CVehicleInterface::~CVehicleInterface( void )
{
    try
    {
        m_bInitialized = false;

        // Explicitly destroy the auto_ptr<> objects; not really needed but done for symmetry and to avoid misunderstanding.

        if ( m_pSocketServer.get() != NULL )
        {
            delete ( m_pSocketServer.release() );
            m_pSocketServer = auto_ptr<CUDPSocket>( NULL );
        }

        if ( m_pRecord.get() != NULL )
        {
            delete ( m_pRecord.release() );
            m_pRecord = auto_ptr<Buffer_t>( NULL );
        }

        // Clear our subscription list.

        m_SubscriberList.clear();

        m_unPort = 0;
    }
    catch ( ... )
    {
        TRACE( _T( "CVehicleData::CVehicleInterface::~CVehicleInterface(), Unspecified exception caught\n" ) );
    }
}

inline
bool CVehicleData::CVehicleInterface::IsInitialized( void ) const
{
    return m_bInitialized;
}

inline
bool CVehicleData::CVehicleInterface::Reset( void )
{
    bool bSuccess = false;

    try
    {
        // Restart the UDP socket server.

        if ( m_pSocketServer.get() != NULL )
        {
            delete ( m_pSocketServer.release() );
            m_pSocketServer = auto_ptr<CUDPSocket>( NULL );
        }

        m_pSocketServer = auto_ptr<CUDPSocket>( new CUDPSocket( CUDPSocket::modeServer, m_unPort, false, 0UL, ReceiveHandler, this ) );
        ASSERT( m_pSocketServer.get() != NULL );

        // Reset the various data buffers but DO NOT alter the client subscription list.

        m_pRecord->Reset();
        m_NavigationFrameQueue.Reset();
        m_EnvironmentFrameQueue.Reset();

        // Reset the time of last update to enable detection of the timeout period expiring.

        __TRY
        {
            m_FlagsCriticalSection.Enter();
            m_ulTimeStampOfLastUpdate = 0UL;
        }
        __FINALLY
        {
            m_FlagsCriticalSection.Leave();
        }
        __ENDFINALLY

        bSuccess = ( m_pSocketServer.get() != NULL );
    }
    catch ( ... )
    {
        bSuccess = false;
        TRACE( _T( "CVehicleData::CVehicleInterface::Reset(), Unspecified exception caught\n" ) );
    }

    return bSuccess;
}

inline
bool CVehicleData::CVehicleInterface::Subscribe( const CVehicleData *pParent )
{
    bool bSuccess = false;

    try
    {
        if ( pParent == NULL )
        {
            ThrowMessage_m( "pParent is NULL" );
        }

        const int iSensorIndex = pParent->SensorIndex();

        if ( m_SubscriberList.empty() || std::find( m_SubscriberList.begin(), m_SubscriberList.end(), iSensorIndex ) == m_SubscriberList.end() )
        {
            CSubscriber Subscriber( iSensorIndex, pParent );

            m_SubscriberList.push_back( Subscriber );

            m_SubscriberList.sort();
        }

        bSuccess = true;
    }
    catch ( ... )
    {
        bSuccess = false;
    }

    return bSuccess;
}

inline
bool CVehicleData::CVehicleInterface::UnSubscribe(  const int &riSensorIndex )
{
    if ( ! m_SubscriberList.empty() )
    {
        SubscriberListIterator_t pSubscriber = std::find( m_SubscriberList.begin(), m_SubscriberList.end(), riSensorIndex );

        if ( pSubscriber->m_iSensorIndex == riSensorIndex )
        {
            m_SubscriberList.erase( pSubscriber );
        }

        if ( ! m_SubscriberList.empty() )
        {
            m_SubscriberList.sort();
        }
    }

    return true;
}

inline
bool CVehicleData::CVehicleInterface::ReceiveHandler(   const void             *pvParam,
                                                        const BYTE             *pbyData,
                                                        const unsigned long    &rulBytes,
                                                        const unsigned long    &rulTimeStamp )
{
    // Here, a false return code means some kind of fatal error; typically an uncaught exception.

    bool bSuccess = false;

    try
    {
        ASSERT( rulBytes >  0UL );
        ASSERT( pbyData  != NULL );

        CVehicleInterface * const pthis = reinterpret_cast<CVehicleInterface *>( const_cast<void *>( pvParam ) );

        if ( pthis != NULL )
        {
            if ( ! pthis->PreProcessAndDispatch( pbyData, rulBytes, rulTimeStamp ) )
            {
                TRACE( _T( "CVehicleData::CVehicleInterface::ReceiveHandler(), PreProcessAndDispatch() failed.\n" ) );
            }

            bSuccess = true;
        }
    }
    catch ( ... )
    {
        bSuccess = false;
    }

    return bSuccess;
}

inline
bool CVehicleData::CVehicleInterface::PreProcessAndDispatch(    const BYTE             *pbyData,
                                                                const unsigned long    &rulBytes,
                                                                const unsigned long    &rulTimeStamp )
{
    // Here we accumulate the data frames and when complete, format a 7k message and dispatch
    // to subscribing clients (CVehicleData sensor objects).

    bool bSuccess = false;           // Assume failure for now.

    try
    {
        __TRY
        {
            m_FlagsCriticalSection.Enter();
            m_ulTimeStampOfLastUpdate = rulTimeStamp;
        }
        __FINALLY
        {
            m_FlagsCriticalSection.Leave();
        }
        __ENDFINALLY

        if ( rulBytes == 0UL )
        {
            bSuccess = true;        // Nothing to do.
        }
        else if ( pbyData == NULL )
        {
            ThrowMessage_m( "pbyData is NULL" );
        }
        else
        {
            // Accumulate the frame.
            
            CBlueFinNetworkFrame Frame( pbyData, rulBytes );

            if ( ! Frame.IsValid() )
            {
                ThrowMessage_m( "BlueFin network frame is invalid" );
            }

            // Valid frame has been received so handle as necessary.

            const CBlueFinNetworkFrame::EDATATYPE eType = Frame.DataType();

            //TRACE( _T( "BlueFin frame type %d (size: %lu)\n" ), static_cast<int>( eType ), rulBytes );

            CFrameQueue *pFrameQueue = NULL;

            try
            {
                switch ( eType )
                {
                    case CBlueFinNetworkFrame::dataTypeNavigation:

                        pFrameQueue = &m_NavigationFrameQueue;
                        break;

                    case CBlueFinNetworkFrame::dataTypeEnvironment:

                        pFrameQueue = &m_EnvironmentFrameQueue;
                        break;

                    default:

                        pFrameQueue = NULL;
                        break;
                }

                if ( pFrameQueue == NULL )
                {
                    ThrowMessage_m( "pFrameQueue is invalid" );
                }

#if 1
#pragma CompileMessage_m( "TODO: RE-ENABLE ONCE CRITICAL PARAMETER HANDLING IS FINALIZED" )
#else
                if ( ! UpdateCriticalParameterTable( Frame, rulTimeStamp ) )
                {
                    TRACE( _T( "UpdateCriticalParameterTable failed\n" ) );
                }
#endif

                ASSERT( m_pRecord.get() != NULL );

                if ( ! pFrameQueue->AddFrame( const_cast<BYTE *>( pbyData ), rulBytes, rulTimeStamp ) )
                {
                    ThrowMessage_m( "pFrameQueue->AddFrame() failed" );
                }

                if ( pFrameQueue->IsComplete() )
                {
                    //TRACE( _T( "BlueFin frame complete (type: %d)\n" ), static_cast<int>( pFrameQueue->FrameType() ) );

                    if ( ! FormatRecord( pFrameQueue ) )
                    {
                        ThrowMessage_m( "FormatRecord() failed" );
                    }

                    // 7k record is complete so dispatch to subscribing sensors for queueing ahead of retrieval.

                    if ( ! NotifySubscribers( m_pRecord->GetAt( 0 ), m_pRecord->Size(), pFrameQueue->Timestamp() ) )
                    {
                        ThrowMessage_m( "NotifySubscribers failed" );
                    }

                    m_pRecord->Reset();
                    pFrameQueue->Reset();

                    bSuccess = true;
                }
                else
                {
                    bSuccess = true;        // Still accumulating but all is ok.
                }
            }
            catch ( ... )
            {
                // Reset the buffers ready to try to reaccumulate a complete set of frames then re-throw the same exception
                // to report it set the appropriate exit code.

                if ( m_pRecord.get() != NULL )
                {
                    m_pRecord->Reset();
                }

                if ( pFrameQueue != NULL )
                {
                    pFrameQueue->Reset();
                }

                // Rethrow the same exception.

                throw;
            }
        }
    }
    catch ( LPCTSTR lpszMessage )
    {
        bSuccess = false;
        TRACE( _T( "CVehicleData::CVehicleInterface::PreProcessAndDispatch(), %s.\n" ), lpszMessage );
    }
    catch ( ... )
    {
        bSuccess = false;
        TRACE( _T( "CVehicleData::CVehicleInterface::PreProcessAndDispatch(), Unspecified exception caught\n" ) );
    }
    
    return bSuccess;
}

inline
bool CVehicleData::CVehicleInterface::NotifySubscribers(    const BYTE             *pbyRecord,
                                                            const unsigned long    &rulBytes,
                                                            const unsigned long    &rulTimeStamp )
{
    bool bSuccess = false;

    try
    {
        ASSERT( rulBytes    >  0UL );
        ASSERT( pbyRecord != NULL );

        if ( ! m_SubscriberList.empty() )
        {
            bSuccess = true;

            for ( SubscriberListIterator_t pSubscriber = m_SubscriberList.begin(); pSubscriber != m_SubscriberList.end(); pSubscriber++ )
            {
                if ( ! pSubscriber->m_pParent->MessageHandler( pbyRecord, rulBytes, rulTimeStamp ) )
                {
                    bSuccess = false;
                }
            }
        }
    }
    catch ( ... )
    {
        bSuccess = false;
    }

    return bSuccess;
}

inline
bool CVehicleData::CVehicleInterface::FormatRecord( const CFrameQueue *pFrameQueue ) const
{
    bool bSuccess = false;

    try
    {
        if ( pFrameQueue == NULL )
        {
            ThrowMessage_m( "pFrameQueue is NULL" );
        }

        // Format the 7k vehicle record comprising of:
        //
        // - Data record frame              (7k header)
        // - Record type header             (BLUEFINRECORDTYPEHEADER)
        // - Record data                    (Queued frame data).
        // - Checksum

        // First, pre-compute anticipated size for DRF header. Later we will explicitly test this assertion.

        const unsigned long ulTotal7kRecordSize =   tagRECORDHEADER7K::Size() +
                                                        tagBLUEFINRECORDTYPEHEADER::Size() +
                                                            pFrameQueue->FrameSize() * pFrameQueue->NumberOfFrames() +    
                                                                Checksum7kSize_m();

        // Firstly, add the 7k record header (DRF).

        RECORDHEADER7K sRecordHeader;

        SetDefaultHeader( &sRecordHeader, pFrameQueue->Timestamp() );

        sRecordHeader.m_ulSize         = ulTotal7kRecordSize;
        sRecordHeader.m_ulRecordType   = recordTypeBlueFinVC;
        sRecordHeader.m_ulDeviceId     = deviceIdBlueFinVC;

        m_pRecord->Add( reinterpret_cast<BYTE *>( &sRecordHeader ), tagRECORDHEADER7K::Size() );

        // Next the record type header.

        BLUEFINRECORDTYPEHEADER sRecordTypeHeader;

        memset( &sRecordTypeHeader, 0x00, tagBLUEFINRECORDTYPEHEADER::Size() );

        sRecordTypeHeader.m_ulTimeStamp      = pFrameQueue->Timestamp();
        sRecordTypeHeader.m_ulNumberOfFrames = pFrameQueue->NumberOfFrames();
        sRecordTypeHeader.m_ulFrameSize      = pFrameQueue->FrameSize();
        sRecordTypeHeader.m_ulDataFormat     = static_cast<unsigned long>( pFrameQueue->FrameType() );

        m_pRecord->Add( reinterpret_cast<BYTE *>( &sRecordTypeHeader ), tagBLUEFINRECORDTYPEHEADER::Size() );

        // The queued frame data.

        m_pRecord->Add( static_cast<BYTE *>( *pFrameQueue ), static_cast<unsigned long>( *pFrameQueue ) );

        // The checksum is not computed for now. Note also, checksum bit flag also set to zero in DRF.

        Checksum7k_t CheckSum = 0;

        m_pRecord->Add( reinterpret_cast<BYTE *>( &CheckSum ), sizeof( Checksum7k_t ) );

        // Total size should be 7k record size.

        ASSERT( m_pRecord->Size() == ulTotal7kRecordSize );

        bSuccess = true;
    }
    catch ( LPCTSTR lpszMessage )
    {
       bSuccess = false;
       TRACE( _T( "CVehicleData::CVehicleInterface::FormatRecord(), %s\n" ), lpszMessage );
    }
    catch ( ... )
    {
       bSuccess = false;
       TRACE( _T( "CVehicleData::CVehicleInterface::FormatRecord(), unspecified exception caught\n" ) );
    }

    return bSuccess;
}

inline
void CVehicleData::CVehicleInterface::SetDefaultHeader( RECORDHEADER7K         *psRecordHeader,
                                                        const unsigned long    &rulTimeStamp ) const
{
    // Set sensible defaults for the 7k command header.

    if ( psRecordHeader != NULL )
    {
        const unsigned long ulRecordHeaderSize = tagRECORDHEADER7K::Size();

        memset( psRecordHeader, 0x00, ulRecordHeaderSize );

        psRecordHeader->m_unVersion                 = un7kDataProtocolVersion_c;                                     // u16 Version of this frame (e.g.: 1, 2, …)
        psRecordHeader->m_unOffset                  = static_cast<unsigned short>( ulRecordHeaderSize - 
                                                        ( 2 * sizeof( unsigned short ) ) );                          // u16 Offset in bytes from the start of the sync pattern to the start of the DATA SECTION. This allows for expansion of the header whilst maintaining backward compatibility.
        psRecordHeader->m_ulSyncPattern             = 0x0000ffff;                                                    // u32 0x0000FFFF
        psRecordHeader->m_ulSize                    = 0UL;                                                           // u32 Size in bytes of this record from the start of the version field to the end of the Checksum. It includes the embedded data size.

        psRecordHeader->m_ulOffsetToOptionalData    = 0UL;                                                           // Data offset          u32 Offset in bytes to optional data field from start of record. Zero implies no optional data.
        psRecordHeader->m_ulOptionalDataIdentifier  = 0UL;                                                           // Data idenfitifer     u32 Identifier for optional data field. Zero for no optional field. This identifier is described with each record type.

        if ( ! CSystemTime::TimeStampTo7kTime( rulTimeStamp, &(psRecordHeader->m_sTime7k) ) )
        {
            TRACE( _T( "CVehicleData::CVehicleInterface::SetDefaultHeader(), Failed to set time stamp" ) );
        }

        psRecordHeader->m_ulRecordType              = 0UL;                                                            // u32 Unique identifier of indicating the type of data embedded in this record.
        psRecordHeader->m_ulDeviceId                = 0UL;                                                            // u32 Identifier of the device that this data pertains.


#if   ( DRF_VERSION_7K == 1 )

        psRecordHeader->m_ulSubsystemId             = 0UL;
        psRecordHeader->m_ulDataSetNumber           = 0UL;                                                            // u32 Data set number.
        psRecordHeader->m_i64PreviousRecord         = (__int64) -1;                                                   // i64 Pointer to the previous record of the same type (in bytes from start of file). This is an optional field for files and shall be -1 if not used.
        psRecordHeader->m_i64NextRecord             = (__int64) -1;                                                   // i64 Pointer to the next record of the same type in bytes from start of file. This is an optional field for files and shall be -1 if not used.

#elif ( DRF_VERSION_7K == 2 )

        psRecordHeader->m_unSubsystemId             = 0U;                                                             // u32 Identifier for the device subsystem
        psRecordHeader->m_ulDataSetNumber           = 0UL;                                                            // u32 Data set number.
        psRecordHeader->m_i64PreviousRecord         = (__int64) -1;                                                   // i64 Pointer to the previous record of the same type (in bytes from start of file). This is an optional field for files and shall be -1 if not used.
        psRecordHeader->m_i64NextRecord             = (__int64) -1;                                                   // i64 Pointer to the next record of the same type in bytes from start of file. This is an optional field for files and shall be -1 if not used.

#elif ( DRF_VERSION_7K >= 5 )

        psRecordHeader->m_unRecordsVersion          = un7kRecordsVersion_c;                                           // u16 Record version within a given verion of the protocol.

#endif

        psRecordHeader->m_ulRecordNumber            = 0UL;                                                            // u32 Sequential record counter.
        psRecordHeader->m_unFlags                   = 0x0000;                                                         // u16 BIT FIELD: Bit 1 - Valid Checksum
    }
}

inline
bool CVehicleData::CVehicleInterface::UpdateCriticalParameterTable( const CBlueFinNetworkFrame  &rFrame,
                                                                    const unsigned long         &rulTimeStamp )
{
    bool bSuccess = true;       // Assume success until we know otherwise.

    try
    {
        CBlueFinVCApp *pApp = GetApp_m();

        if ( pApp == NULL )
        {
            ThrowMessage_m( "pApp is NULL" );
        }

        m_CriticalParameters.clear();
        CRITICALPARAMENTRY sEntry;

        switch ( rFrame.DataType() )
        {
            case CBlueFinNetworkFrame::dataTypeNavigation:

                {
                    const BLUEFINNAVIGATIONMESSAGE *pNavData = static_cast<const BLUEFINNAVIGATIONMESSAGE *>( rFrame );

                    typedef struct tagPARAM
                    {
                        unsigned long m_ulQualityFlagMask;
                        unsigned long m_ulParameterId;
                    }
                    PARAM;
                    
                    static PARAM sParamsToCheck[] = {
                                                        { validFlagBitNavSpeed,    criticalParameterVesselSpeed },
                                                        { validFlagBitNavAltitude, criticalParameterAltitude    },
                                                        { validFlagBitNavYaw,      criticalParameterYaw         },
                                                        { validFlagBitNavPitch,    criticalParameterPitch       },
                                                        { validFlagBitNavRoll,     criticalParameterRoll        }
                                                    };

                    for ( int iParam = 0UL; iParam < static_cast<int>( DimOf_m( sParamsToCheck ) ); iParam++ )
                    {
                        if ( ( pNavData->m_ulQualityMetrics & static_cast<unsigned long>( 1 << sParamsToCheck[ iParam ].m_ulQualityFlagMask ) ) != 0UL )
                        {
                            sEntry.m_ulParameterId       = sParamsToCheck[ iParam ].m_ulParameterId;
                            sEntry.m_ulTimeStampOfUpdate = rulTimeStamp;

                            switch ( sParamsToCheck[ iParam ].m_ulParameterId )
                            {
                                case criticalParameterVesselSpeed:

                                    sEntry.m_dValue = static_cast<double>( pNavData->m_fSpeed );
                                    break;

                                case criticalParameterAltitude:

                                    sEntry.m_dValue = pNavData->m_dAltitude;
                                    break;

                                case criticalParameterYaw:

                                    sEntry.m_dValue = static_cast<double>( pNavData->m_fYaw );
                                    break;

                                case criticalParameterPitch:

                                    sEntry.m_dValue = static_cast<double>( pNavData->m_fPitch );
                                    break;

                                case criticalParameterRoll:

                                    sEntry.m_dValue = static_cast<double>( pNavData->m_fRoll );
                                    break;
                            }

                            m_CriticalParameters.push_back( sEntry );
                        }
                    }
                }

                break;

            case CBlueFinNetworkFrame::dataTypeEnvironment:

                {
                    const BLUEFINENVIRONMENTMESSAGE *pEnvData = static_cast<const BLUEFINENVIRONMENTMESSAGE *>( rFrame );
                
                    if ( ( pEnvData->m_ulQualityMetrics & static_cast<unsigned long>( 1 << validFlagBitEnvSoundSpeed ) ) != 0UL )
                    {
                        sEntry.m_dValue              = static_cast<double>( pEnvData->m_fSoundVelocity );
                        sEntry.m_ulParameterId       = static_cast<unsigned long>( criticalParameterSoundSpeed );
                        sEntry.m_ulTimeStampOfUpdate = rulTimeStamp;

                        m_CriticalParameters.push_back( sEntry );
                    }
                }
                
                break;

            default:

                TRACE( _T( " CVehicleData::CVehicleInterface::UpdateCriticalParameterTable(), Unknown frame type\n" ) );
                break;
        }

        if ( m_CriticalParameters.empty() )
        {
            bSuccess = true;
            TRACE( _T( "CVehicleData::CVehicleInterface::UpdateCriticalParameterTable(), No valid critical parameters to update\n" ) );
        }
        else if ( pApp->UpdateCriticalParameters(   static_cast<unsigned long>( m_CriticalParameters.size() ),
                                                        &m_CriticalParameters[ 0 ],
                                                            rulTimeStamp ) )
        {
            bSuccess = true;
        }
        else
        {
            bSuccess = false;
            TRACE( _T( "CVehicleData::CVehicleInterface::UpdateCriticalParameterTable(), pApp->UpdateCriticalParameters failed\n" ) );
        }
    }
    catch ( LPCTSTR lpszMessage )
    {
        bSuccess = false;
        TRACE( _T( " CVehicleData::CVehicleInterface::UpdateCriticalParameterTable(), %s.\n" ), lpszMessage );
    }
    catch ( ... )
    {
        bSuccess = false;
        TRACE( _T( " CVehicleData::CVehicleInterface::UpdateCriticalParameterTable(), Unspecfied exception.\n" ) );
    }

    return bSuccess;
}

inline
bool CVehicleData::CVehicleInterface::IsTimedOut( void ) const
{
    unsigned long ulLastUpdate = 0UL;

    __TRY
    {
        m_FlagsCriticalSection.Enter();
        ulLastUpdate = m_ulTimeStampOfLastUpdate;
    }
    __FINALLY
    {
        m_FlagsCriticalSection.Leave();
    }
    __ENDFINALLY

    return ( ( CSystemTime::GetTickCount() - ulLastUpdate ) >= m_ulDataTimeOut );
}

///////////////////////////////////////////////////////////////////////////////
// CVehicleData::CHealthStatus internal helper implementation.

CVehicleData::CHealthStatus::CHealthStatus  ( void )
                            :m_iMinFailCount( 0 ),
                             m_iMaxFailCount( 5 )
{
    __TRY
    {
        m_StateGuard.Enter();

        m_bFailed    = false;
        m_iFailCount = 0;
    }
    __FINALLY
    {
        m_StateGuard.Leave();
    }
    __ENDFINALLY
}

CVehicleData::CHealthStatus::~CHealthStatus( void )
{
    __TRY
    {
        m_StateGuard.Enter();

        m_bFailed    = false;
        m_iFailCount = 0;
    }
    __FINALLY
    {
        m_StateGuard.Leave();
    }
    __ENDFINALLY
}

bool CVehicleData::CHealthStatus::IsResponsive( void ) const
{
    bool bResponsive = true;

    __TRY
    {
        m_StateGuard.Enter();
        bResponsive = ! m_bFailed;
    }
    __FINALLY
    {
        m_StateGuard.Leave();
    }
    __ENDFINALLY

    return bResponsive;
}

bool CVehicleData::CHealthStatus::UpdateStatus( const bool &rbResponding,
                                                const bool &rbForceStatus ) // = false.
{
    bool bStatusChanged = false;

    __TRY
    {
        m_StateGuard.Enter();

        if ( rbForceStatus )
        {
            bStatusChanged = false;
            m_bFailed      = ! rbResponding;

            if ( m_bFailed )
            {
                m_iFailCount = m_iMaxFailCount;
            }
            else
            {
                m_iFailCount = m_iMinFailCount;
            }
        }
        else
        {
            // Increment or decrement the fail counter (as appropriate) then ceil the result on the range [m_iMinFailCount, m_iMaxFailCount]. This
            // gives a hysteresis hence smooths the transition between fail states.

            m_iFailCount = max( min( ( ( rbResponding ) ? --m_iFailCount : ++m_iFailCount ), m_iMaxFailCount ), m_iMinFailCount );

            if ( m_iFailCount == m_iMaxFailCount )
            {
                bStatusChanged = ( m_bFailed == false );
                m_bFailed = true;
            }
            else if ( m_iFailCount == m_iMinFailCount )
            {
                bStatusChanged = ( m_bFailed == true );
                m_bFailed = false;
            }
        }
    }
    __FINALLY
    {
        m_StateGuard.Leave();
    }
    __ENDFINALLY

    return bStatusChanged;
}