//
//  Copyright © 2002, 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:   CriticalParameterTable.cpp
//
//  Project:    6046
//
//  Author(s):  W. Arcus
//
//  Purpose:    Implements the CCriticalParameterTable class in order to support requirement 3.2.13 of 
//              the Phase II Payload Controller SRS. Specifically, it implements an event driven critical 
//              parameter data table that is maintained for subsequent routing to subsribing sensors.
//              For example, sound speed to the RESON 7k sonar.
//
//              The following core features are supported:
//
//              i.  Allow clients to register and update critical parameters in the table.
//              ii. Asynchonously notifies subscribing clients when the appropriate parameters have changed.
//
//  Notes:      
//

#include "StdAfx.h"
#include "CriticalParameterTable.h"

#include <algorithm>

///////////////////////////////////////////////////////////////////////////////
// CriticalParameterTable class implementtion.

// Static member initialization.

CRITICALPARAMENTRY CCriticalParameterTable::m_asPrimaryTable[ criticalParameterMaxParameters ] =
{
    { criticalParameterSoundSpeed,          0UL,    0.0 },              // Sound speed  (m/s).
    { criticalParameterVesselSpeed,         0UL,    0.0 },              // Vessel speed (m/s).
    { criticalParameterAltitude,            0UL,    0.0 },              // Altitude     (m).
    { criticalParameterHeading,             0UL,    0.0 },              // Heading      (rad).
    { criticalParameterYaw,                 0UL,    0.0 },              // Yaw          (rad).
    { criticalParameterHeave,               0UL,    0.0 },              // Heave        (m).
    { criticalParameterPitch,               0UL,    0.0 },              // Pitch        (rad).
    { criticalParameterRoll,                0UL,    0.0 },              // Roll         (rad).

    { criticalParameterPulseRepPeriod0,     0UL,    0.0 },              // Trigger pulse rep. period for sensor index 0 (s).
    { criticalParameterPulseRepPeriod1,     0UL,    0.0 },
    { criticalParameterPulseRepPeriod2,     0UL,    0.0 },
    { criticalParameterPulseRepPeriod3,     0UL,    0.0 },
    { criticalParameterPulseRepPeriod4,     0UL,    0.0 },
    { criticalParameterPulseRepPeriod5,     0UL,    0.0 },
    { criticalParameterPulseRepPeriod6,     0UL,    0.0 },
    { criticalParameterPulseRepPeriod7,     0UL,    0.0 },
    { criticalParameterPulseRepPeriod8,     0UL,    0.0 },
    { criticalParameterPulseRepPeriod9,     0UL,    0.0 },
    { criticalParameterPulseRepPeriod10,    0UL,    0.0 },
    { criticalParameterPulseRepPeriod11,    0UL,    0.0 },
    { criticalParameterPulseRepPeriod12,    0UL,    0.0 },
    { criticalParameterPulseRepPeriod13,    0UL,    0.0 },
    { criticalParameterPulseRepPeriod14,    0UL,    0.0 },
    { criticalParameterPulseRepPeriod15,    0UL,    0.0 }               // Trigger pulse rep. period for sensor index 16 (s).
};

///////////////////////////////////////////////////////////////////////////////
// Public services.

using std::find;
using std::vector;

CCriticalParameterTable::CCriticalParameterTable( void )

                        :CBaseThread( INFINITE, THREAD_PRIORITY_NORMAL ),

                         m_iInvalidSensorIndex( -1 ),
                         m_ulNumberOfParameterEntries( static_cast<unsigned long>( DimOf_m( m_asPrimaryTable ) ) ),
                         m_ulPrimaryTableSizeInBytes( m_ulNumberOfParameterEntries * sizeof( CRITICALPARAMENTRY ) )
{
    m_DataGuard.WaitToWrite();
    ::memcpy( &m_asSecondaryTable[ 0 ], &m_asPrimaryTable[ 0 ], m_ulPrimaryTableSizeInBytes );
    m_DataGuard.DoneWriting();

    m_ListGuard.Enter();
    m_SubscribingSensorList.clear();
    m_ListGuard.Leave();

    m_hTerminateEvent = CreateEvent( NULL, TRUE, FALSE, NULL );

    if ( ResumeThread() == m_dwResumeFailCode )
    {
        TRACE( _T( "CCriticalParameterTable::CCriticalParameterTable(), ResumeThread() failed\n" ) );
    }
}

CCriticalParameterTable::~CCriticalParameterTable( void )
{
    try
    {
        if ( m_hTerminateEvent != NULL )
        {
            SetEvent( m_hTerminateEvent );
        }

        if ( ! CBaseThread::WaitForThreadTermination( 2000 ) )
        {
            TRACE( _T( "CCriticalParameterTable::~CCriticalParameterTable(), WaitForThreadTermination() timed out or failed\n" ) );
        }

        m_ListGuard.Enter();
        m_SubscribingSensorList.clear();
        m_ListGuard.Leave();

        if ( m_hTerminateEvent != NULL )
        {
            CloseHandle( m_hTerminateEvent );
            m_hTerminateEvent = NULL;
        }
    }
    catch ( ... )
    {
        TRACE( _T( "CCriticalParameterTable::~CCriticalParameterTable(), unspecified exception caught.\n" ) );
    }
}

bool CCriticalParameterTable::Subscribe(    const int                          &riSensorIndex,
                                            PFN_CRITICAL_PARAM_UPDATE           pfnCriticalParamCallback,
                                            void                               *pvParam,
                                            ParameterList_t                    *pParameters )
{
    bool bSuccess = false;

    if ( riSensorIndex > m_iInvalidSensorIndex )
    {
        bool bAdded = false;

        m_ListGuard.Enter();

        try
        {
            if ( m_SubscribingSensorList.empty() )
            {
                CSensorItem Item( riSensorIndex, pfnCriticalParamCallback, pvParam );

                m_SubscribingSensorList.push_back( Item );

                bAdded = true;
            }
            else
            {
                SubscribingSensorListIterator_t pSensor = find( m_SubscribingSensorList.begin(), m_SubscribingSensorList.end(), riSensorIndex );

                if ( pSensor == m_SubscribingSensorList.end() )
                {
                    CSensorItem Item( riSensorIndex, pfnCriticalParamCallback, pvParam );

                    m_SubscribingSensorList.push_back( Item );

                    bAdded = true;
                }
            }

            if ( bAdded )
            {
                (m_SubscribingSensorList.back()).Subscribe( ( pParameters == NULL ), pParameters );
            }

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

        m_ListGuard.Leave();
    }

    return bSuccess;
}

bool CCriticalParameterTable::Unsubscribe( const int &riSensorIndex )
{
    bool bSuccess = false;

    if ( riSensorIndex > m_iInvalidSensorIndex )
    {
        m_ListGuard.Enter();

        try
        {
            if ( ! m_SubscribingSensorList.empty() )
            {
                SubscribingSensorListIterator_t pSensor = find( m_SubscribingSensorList.begin(), m_SubscribingSensorList.end(), riSensorIndex );

                if ( pSensor != m_SubscribingSensorList.end() )
                {
                    m_SubscribingSensorList.erase( pSensor );
                }
            }

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

        m_ListGuard.Leave();
    }

    return bSuccess;
}

bool CCriticalParameterTable::UpdateParameters( const unsigned long         &rulParametersToUpdate,
                                                const CRITICALPARAMENTRY    *psParameterEntry,
                                                const unsigned long         &rulTimeStamp )
{
    bool bSuccess = false;

    ASSERT( rulParametersToUpdate >  0UL );
    ASSERT( psParameterEntry      != NULL );

    m_DataGuard.WaitToWrite();

    try
    {
        for ( unsigned long ulTableEntry = 0UL; ulTableEntry < m_ulNumberOfParameterEntries; ulTableEntry++ )
        {
            for ( unsigned long ulParameter = 0UL; ulParameter < rulParametersToUpdate; ulParameter++ )
            {
                if ( m_asPrimaryTable[ ulTableEntry ].m_ulParameterId == psParameterEntry[ ulParameter ].m_ulParameterId )
                {
                    m_asPrimaryTable[ ulTableEntry ].m_ulTimeStampOfUpdate = rulTimeStamp;
                    m_asPrimaryTable[ ulTableEntry ].m_dValue              = psParameterEntry[ ulParameter ].m_dValue;
                    bSuccess = true;
                    break;
                }
            }
        }
    }
    catch ( ... )
    {
        bSuccess = false;
    }

    m_DataGuard.DoneWriting();

    if ( bSuccess )
    {
        bool bQueued = false;

        m_ListGuard.Enter();

        try
        {
            for ( SubscribingSensorListIterator_t pSensor = m_SubscribingSensorList.begin(); pSensor != m_SubscribingSensorList.end(); pSensor++ )
            {
                if ( pSensor->QueueCallback( rulParametersToUpdate, psParameterEntry ) )
                {
                    bQueued = true;
                }
            }
        }
        catch ( ... )
        {
            bSuccess = false;
        }

        m_ListGuard.Leave();

        if ( bSuccess && bQueued )
        {
            if ( ! PostThreadMessage( messageCriticalParamUpdate ) )
            {
                bSuccess = false;
                TRACE( _T( "CCriticalParameterTable::UpdateParameter(), PostThreadMessage() failed\n" ) );
            }
        }
    }

    return bSuccess;
}

///////////////////////////////////////////////////////////////////////////////
// Private members.

void CCriticalParameterTable::WatchCycle( void )
{
    // Base class override for main thread processing.

    try
    {
        DWORD dwSignalState;

        while ( IsActive() )
        {
            // Block this thread and wait for a significant event to occur, namely: a critical parameter update 
            // or a thread termination message.

            HANDLE ahWaitObjects[] = { m_hTerminateEvent };

            dwSignalState = ::MsgWaitForMultipleObjects( 1,                     // Number of waitable kernel objects (events, timers etc) to wait for.
                                                         &ahWaitObjects[ 0 ],   // Handle array.
                                                         FALSE,                 // Wait for any one of the specified event types.
                                                         m_dwDwellPeriod,       // Timeout processing period in ms.
                                                         QS_ALLINPUT );         // Unblock on any message in the thread's queue.

            if ( IsInactive() )
            {
                break;
            }
            else if ( dwSignalState == WAIT_OBJECT_0 )
            {
                SetActive( FALSE );
            }
        }
    }
    catch ( ... )
    {
        TRACE( _T( "CCriticalParameterTable::WatchCycle(), Unspecified exception caught\n" ) );
    }

    TRACE( _T( "CCriticalParameterTable::WatchCycle(), Thread terminating.\n" ) );
}

BOOL CCriticalParameterTable::ProcessMessage( MSG *psMsg )
{
    BOOL bMessageProcessed = FALSE;

    //TRACE( _T( "CCriticalParameterTable::ProcessMessage(), Message %u recieved.\n" ), psMsg->message );

    switch ( psMsg->message )
    {
        case messageCriticalParamUpdate:

            UpdateSubscribingClients();
            bMessageProcessed = TRUE;
            break;

        default:

            // Give the base class the chance to handle the unknown or uninteresting message.

            bMessageProcessed = CBaseThread::ProcessMessage( psMsg );
            break;
    }

    return bMessageProcessed;
}

inline
void CCriticalParameterTable::UpdateSubscribingClients( void )
{
    bool bSubscribingSensors = false;

    m_ListGuard.Enter();

    try
    {
        bSubscribingSensors = ! m_SubscribingSensorList.empty();
    }
    catch ( ... )
    {
        bSubscribingSensors = false;
    }

    m_ListGuard.Leave();

    if ( bSubscribingSensors )
    {
        bool bSuccess = true;

        m_DataGuard.WaitToRead();

        try
        {
            ::memcpy( &m_asSecondaryTable[ 0 ], &m_asPrimaryTable[ 0 ], m_ulPrimaryTableSizeInBytes );
        }
        catch ( ... )
        {
            bSuccess = false;
        }

        m_DataGuard.DoneReading();

        if ( bSuccess )
        {
            m_ListGuard.Enter();

            try
            {
                for ( SubscribingSensorListIterator_t pSensor = m_SubscribingSensorList.begin(); pSensor != m_SubscribingSensorList.end(); pSensor++ )
                {
                    if ( pSensor->IsCallbackQueued() )
                    {
                        ( *pSensor )( m_ulNumberOfParameterEntries, &m_asSecondaryTable[ 0 ] );
            
                        pSensor->CancelCallback();
                    }
                }
            }
            catch ( ... )
            {
                bSuccess = false;
            }

            m_ListGuard.Leave();
        }

        ASSERT( bSuccess );
    }
}

///////////////////////////////////////////////////////////////////////////////
// CSensorItem class implementation -- an internal helper.

CCriticalParameterTable::CSensorItem::CSensorItem(  const int                      &riSensorIndex,
                                                    PFN_CRITICAL_PARAM_UPDATE       pfnCallback,
                                                    void                           *pvParam )
{
    m_iSensorIndex    = riSensorIndex;
    m_pfnCallback     = pfnCallback;
    m_pvParam         = pvParam;

    m_bSubscribeAll   = true;
    m_bQueued         = false;

    m_SubscriptionIds.empty();
}

CCriticalParameterTable::CSensorItem::CSensorItem( const CCriticalParameterTable::CSensorItem &rRhs )
{
    m_iSensorIndex    = rRhs.m_iSensorIndex;
    m_pfnCallback     = rRhs.m_pfnCallback;
    m_pvParam         = rRhs.m_pvParam;

    m_bSubscribeAll   = rRhs.m_bSubscribeAll;
    m_bQueued         = rRhs.m_bQueued;

    m_SubscriptionIds = rRhs.m_SubscriptionIds;
}

CCriticalParameterTable::CSensorItem::~CSensorItem( void )
{
    try
    {
        m_iSensorIndex    = -1;
        m_pfnCallback     = NULL;
        m_pvParam         = NULL;

        m_bSubscribeAll   = false;
        m_bQueued         = false;

        m_SubscriptionIds.clear();
    }
    catch ( ... )
    {
        TRACE( _T( "CCriticalParameterTable::CSensorItem::~CSensorItem(), Unspecified exception caught\n" ) );
    }
}

CCriticalParameterTable::CSensorItem & 
CCriticalParameterTable::CSensorItem::operator = ( const CCriticalParameterTable::CSensorItem &rRhs )
{
    if ( &rRhs != this )
    {
        m_iSensorIndex    = rRhs.m_iSensorIndex;
        m_pfnCallback     = rRhs.m_pfnCallback;
        m_pvParam         = rRhs.m_pvParam;

        m_bSubscribeAll   = rRhs.m_bSubscribeAll;
        m_bQueued         = rRhs.m_bQueued;

        m_SubscriptionIds = rRhs.m_SubscriptionIds;
    }

    return *this;
}

bool CCriticalParameterTable::CSensorItem::operator == ( const int &riSensorIndex ) const
{
    return ( m_iSensorIndex == riSensorIndex );
}

bool CCriticalParameterTable::CSensorItem::operator < ( const CSensorItem &rRhs ) const
{
    return ( m_iSensorIndex == rRhs.m_iSensorIndex );
}

int CCriticalParameterTable::CSensorItem::SensorIndex( void ) const
{
    return m_iSensorIndex;
}

void CCriticalParameterTable::CSensorItem::Subscribe(   const bool             &rbSubscribeAll,
                                                        const ParameterList_t  *pParameters )
{
    m_bSubscribeAll = rbSubscribeAll;

    if ( ! rbSubscribeAll )
    {
        if ( ( pParameters != NULL ) && ( ! pParameters->empty() ) )
        {
            m_SubscriptionIds = *pParameters;
        }
        else
        {
            m_SubscriptionIds.clear();
        }
    }
}

bool CCriticalParameterTable::CSensorItem::QueueCallback(   const unsigned long            &rulNumberOfUpdatedParameters,
                                                            const CRITICALPARAMENTRY  *     psParamTable )
{
    if ( ( rulNumberOfUpdatedParameters > 0UL ) &&
         ( psParamTable != NULL )               && 
         ( ! m_SubscriptionIds.empty() )         )
    {
        for ( unsigned long ulPar = 0UL; ulPar < rulNumberOfUpdatedParameters; ulPar++ )
        {
            if ( find( m_SubscriptionIds.begin(), m_SubscriptionIds.end(), static_cast<ECRITICALPARAMETERID>( psParamTable[ ulPar ].m_ulParameterId ) ) != m_SubscriptionIds.end() )
            {
                m_bQueued = true;
                break;
            }
        }
    }

    return m_bQueued;
}

void CCriticalParameterTable::CSensorItem::CancelCallback( void )
{
    m_bQueued = false;
}

bool CCriticalParameterTable::CSensorItem::IsCallbackQueued( void ) const
{
    return m_bQueued;
}

void CCriticalParameterTable::CSensorItem::operator ()  (   const unsigned long          &rulNumberOfParameterEntries,
                                                            CRITICALPARAMENTRY  * const   psParamTable )
{
    try
    {
        ASSERT( m_pfnCallback != NULL );

        if ( m_pfnCallback != NULL )
        {
            ( m_pfnCallback ) ( m_pvParam, m_iSensorIndex, rulNumberOfParameterEntries, psParamTable );
        }
    }
    catch ( ... )
    {
        TRACE( _T( "CCriticalParameterTable::CSensorItem::operator(), Unspecified exception caught\n" ) );
    }
}