// $Header: https://oberon.oapd.inaf.it/shins/trunk/shins/src/tiptilt_ctrl/bcu/Interface/Sharknir/Bcu/OverallDiagnosticRecordRingBuffer.cpp 3851 2022-03-10 14:05:11Z depa $

#include "tiptilt_ctrl/bcu/Interface/Sharknir/Bcu/OverallDiagnosticRecordRingBuffer.h"

#include "tiptilt_ctrl/bcu/util/Assertions.h"

#include <Nice/Util/Exception.h>
//#include <Nice/Log.h>


#include <boost/foreach.hpp>
#include <boost/make_shared.hpp>

#include <algorithm>
#include <cassert>
#include <cstdlib>
#include <limits>


using boost::make_shared;

#define SHARKNIR_BCU_ODR_RING_BUFFER_LOCK() Nice::Monitor::Lock lock(monitor_);

#ifdef LOG_NAME
#undef LOG_NAME
#define LOG_NAME "OverallDiagnosticRecordRingBuffer"
#endif

namespace Sharknir { namespace Bcu {


#define triggerAlarmsFor(alarmType)                       \
{                                                         \
    ArrivalAlarmContainer staleCounters;                  \
    BOOST_FOREACH(ArrivalAlarmPtr each, arrivalCounters_) \
    {                                                     \
        if (NULL == each-> alarmType)                     \
        {                                                 \
            continue;                                     \
        }                                                 \
        assert(*each-> alarmType > 0);                    \
        --(*each-> alarmType);                            \
        if (0 == (*each-> alarmType))                     \
        {                                                 \
            monitor_.notifyAll();                         \
            staleCounters.push_back(each);                \
        }                                                 \
    }                                                     \
                                                          \
    BOOST_FOREACH(ArrivalAlarmPtr each, staleCounters)    \
    {                                                     \
        unregisterArrivalAlarm(each);                     \
    }                                                     \
}

OverallDiagnosticRecordRingBuffer::OverallDiagnosticRecordRingBuffer(
        size_t capacity, size_t delayLimitInFrameCounters) :
	ring_(capacity)
{
	SHARKNIR_PRECONDITION(delayLimitInFrameCounters < std::numeric_limits<FrameCounter>::max(),
       "Delay limit must be lower than frame counter limit");
	SHARKNIR_PRECONDITION(capacity >= delayLimitInFrameCounters,
		"Capacity must be larger or equal than delay limit");

    delayLimitInFrameCounters_= delayLimitInFrameCounters;
    highestReceivedFrameCounter_= 0;
    lastReceivedAsmRecordFrameCounter_= 0;
    asmRecordSkipCounter_= 0;
    duplicatedSlopeRecordsReceivedCounter_= 0;
    duplicatedCcdRecordsReceivedCounter_= 0;
    duplicatedAsmRecordsReceivedCounter_= 0;
    asmRecordsInsertedCounter_= 0;
    ccdRecordsInsertedCounter_= 0;
    slopeRecordsInsertedCounter_= 0;
    lateAsmRecordCounter_= 0;
    lateCcdRecordCounter_= 0;
    lateSlopeRecordCounter_= 0;
}

size_t OverallDiagnosticRecordRingBuffer::getCapacity()
{
    SHARKNIR_BCU_ODR_RING_BUFFER_LOCK()
    return ring_.capacity();
}

size_t OverallDiagnosticRecordRingBuffer::getSize()
{
    SHARKNIR_BCU_ODR_RING_BUFFER_LOCK()
    return ring_.size();
}

size_t OverallDiagnosticRecordRingBuffer::getNumOfDuplicatedSlopeRecords()
{
    SHARKNIR_BCU_ODR_RING_BUFFER_LOCK()
    return duplicatedSlopeRecordsReceivedCounter_;
}

size_t OverallDiagnosticRecordRingBuffer::getNumOfDuplicatedCcdRecords()
{
    SHARKNIR_BCU_ODR_RING_BUFFER_LOCK()
    return duplicatedCcdRecordsReceivedCounter_;
}

size_t OverallDiagnosticRecordRingBuffer::getNumOfDuplicatedAsmRecords()
{
    SHARKNIR_BCU_ODR_RING_BUFFER_LOCK()
    return duplicatedAsmRecordsReceivedCounter_;
}

size_t OverallDiagnosticRecordRingBuffer::getNumOfInsertedSlopeRecords()
{
    SHARKNIR_BCU_ODR_RING_BUFFER_LOCK()
    return slopeRecordsInsertedCounter_;
}

size_t OverallDiagnosticRecordRingBuffer::getNumOfInsertedCcdRecords()
{
    SHARKNIR_BCU_ODR_RING_BUFFER_LOCK()
    return ccdRecordsInsertedCounter_;
}

size_t OverallDiagnosticRecordRingBuffer::getNumOfInsertedAsmRecords()
{
    SHARKNIR_BCU_ODR_RING_BUFFER_LOCK()
    return asmRecordsInsertedCounter_;
}

FrameCounter
OverallDiagnosticRecordRingBuffer::getHighestReceivedFrameCounter()
{
    return highestReceivedFrameCounter_;
}

size_t OverallDiagnosticRecordRingBuffer::getNumOfLateSlopeRecords()
{
    SHARKNIR_BCU_ODR_RING_BUFFER_LOCK()
    return lateSlopeRecordCounter_;
}

size_t OverallDiagnosticRecordRingBuffer::getNumOfLateCcdRecords()
{
    SHARKNIR_BCU_ODR_RING_BUFFER_LOCK()
    return lateCcdRecordCounter_;
}

size_t OverallDiagnosticRecordRingBuffer::getNumOfLateAsmRecords()
{
    SHARKNIR_BCU_ODR_RING_BUFFER_LOCK()
    return lateAsmRecordCounter_;
}

void OverallDiagnosticRecordRingBuffer::clear()
{
    SHARKNIR_BCU_ODR_RING_BUFFER_LOCK()
    highestReceivedFrameCounter_= 0;
    ring_.clear();
}

void OverallDiagnosticRecordRingBuffer::insertSlopeRecord(const FrameCounter frameCounter,
        const Basda::Microgate::ByteBufferPtr& buffer)
{
    SHARKNIR_BCU_ODR_RING_BUFFER_LOCK()

    checkBuffer(buffer);
    OverallDiagnosticRecordPtr overallRecord= findRecordFor(frameCounter);
    assignBuffer(overallRecord->slopeRecord, buffer,
            duplicatedSlopeRecordsReceivedCounter_);
    slopeRecordsInsertedCounter_++;
    triggerAlarmsFor(nSlopeRecords);

    detectLateFrameCounter(frameCounter, "slope record",
            &lateSlopeRecordCounter_);
}

void OverallDiagnosticRecordRingBuffer::insertCcdRecord(const FrameCounter frameCounter,
        const Basda::Microgate::ByteBufferPtr& buffer)
{
    SHARKNIR_BCU_ODR_RING_BUFFER_LOCK()

    checkBuffer(buffer);
    OverallDiagnosticRecordPtr overallRecord= findRecordFor(frameCounter);
    assignBuffer(overallRecord->ccdRecord, buffer,
            duplicatedCcdRecordsReceivedCounter_);
    ccdRecordsInsertedCounter_++;
    triggerAlarmsFor(nCcdRecords);

    detectLateFrameCounter(frameCounter, "CCD record",
            &lateCcdRecordCounter_);
}

#define ODRB_duplicatedAsmSkipFrameCounterMessage()                           \
        "Duplicated ASM record is discarded "                                 \
            "(frame counter: " << frameCounter <<                             \
            ", skip counter: " << asmRecordSkipCounter_ <<                    \
            ", duplicate counter: " <<                                        \
            duplicatedAsmRecordsReceivedCounter_ << ")."                      \

#define	ODRB_skipDuplicatedAsmFrameCounter(frameCounter)                      \
    if (lastReceivedAsmRecordFrameCounter_ == frameCounter &&                 \
        asmRecordsInsertedCounter_ != 0)                                      \
    {                                                                         \
        if (asmRecordSkipCounter_ % 1000 == 0)                                \
        {                                                                     \
            W_LOG(ODRB_duplicatedAsmSkipFrameCounterMessage());               \
        }                                                                     \
        else                                                                  \
        {                                                                     \
            D_LOG(ODRB_duplicatedAsmSkipFrameCounterMessage());               \
        }                                                                     \
        duplicatedAsmRecordsReceivedCounter_++;                               \
        asmRecordSkipCounter_++;                                              \
        return;                                                               \
    }                                                                         \
    else                                                                      \
    {                                                                         \
        lastReceivedAsmRecordFrameCounter_= frameCounter;                     \
        asmRecordSkipCounter_= 0;                                             \
    }


void OverallDiagnosticRecordRingBuffer::insertAsmRecord(
		const FrameCounter frameCounter,
        const Basda::Microgate::ByteBufferPtr& buffer)
{
    SHARKNIR_BCU_ODR_RING_BUFFER_LOCK()

	ODRB_skipDuplicatedAsmFrameCounter(frameCounter);
    checkBuffer(buffer);
    OverallDiagnosticRecordPtr overallRecord= findRecordFor(frameCounter);
    assignBuffer(overallRecord->asmRecord, buffer,
            duplicatedAsmRecordsReceivedCounter_);
    asmRecordsInsertedCounter_++;
    highestReceivedFrameCounter_= std::max(
            highestReceivedFrameCounter_, frameCounter);

    detectLateFrameCounter(frameCounter, "ASM record", &lateAsmRecordCounter_);
}

#define ODR_FOREACH_RECORD_BEGIN(each, N_MAX_RECORDS)         \
    {                                                         \
        CircularBufferType::iterator end= ring_.begin() +     \
            std::min(ring_.size(), (N_MAX_RECORDS));          \
        assert(ring_.begin() + ring_.size() == ring_.end());  \
        for (CircularBufferType::iterator it= ring_.begin();  \
             it != end;  ++it)                                \
        {                                                     \
            const OverallDiagnosticRecordPtr each= *it;


#define ODR_FOREACH_RECORD_END() \
         }                       \
     }

SeqOverallDiagnosticRecordPtr
OverallDiagnosticRecordRingBuffer::getPastOverallDiagnosticRecords(
        const size_t nMaxRecords)
{
    SHARKNIR_BCU_ODR_RING_BUFFER_LOCK()

    const size_t numOfMaxRecords= checkAndAdjustNumOfRequestedRecords(
                                                               nMaxRecords);
    OverallDiagnosticRecordPtrContainer reversedResult;
    reversedResult.reserve(numOfMaxRecords);

    ODR_FOREACH_RECORD_BEGIN(each, numOfMaxRecords)
        assert(0 != each);
        reversedResult.push_back(each);
    ODR_FOREACH_RECORD_END()

    return reverseContainer(&reversedResult);
}

#define waitForFutureOverallDiagnosticRecords(alarmType, nRecords, timeout) \
{                                                                           \
    const Nice::Time to= (timeout);                                         \
    const size_t numOfRecords= (nRecords);                                  \
    ArrivalAlarmPtr alarm(new ArrivalAlarm);                                \
    alarm-> alarmType= new size_t(numOfRecords);                            \
    registerArrivalAlarm(alarm);                                            \
                                                                            \
    const Nice::Date endTime= Nice::Date::now() + to;                       \
    while (0 < (*alarm-> alarmType))                                        \
    {                                                                       \
        const Nice::Time remainingTime= computeTimeoutIfAny(endTime, to);   \
        const bool timeoutAvoided= monitor_.timedWait(remainingTime);       \
        if (not timeoutAvoided)                                             \
        {                                                                   \
            E_THROW(OverallDiagnosticRecordRingBufferException,             \
                    "Timeout of " << timeout <<                             \
                    " occurred when waiting for " << numOfRecords <<        \
                    " records.");                                           \
        }                                                                   \
    }                                                                       \
}

SeqOverallDiagnosticRecordPtr
OverallDiagnosticRecordRingBuffer::getFutureOverallDiagnosticRecords(
            const size_t nRecords, const Nice::Time& timeout)
{
    SHARKNIR_BCU_ODR_RING_BUFFER_LOCK()
    waitForFutureOverallDiagnosticRecords(nFrameCounters, nRecords, timeout)
    return getPastOverallDiagnosticRecords(nRecords);
}

#define checkBreakCondition(detectionCounter) \
    --detectionCounter;                       \
    if (0 == detectionCounter)                \
    {                                         \
        break;                                \
    }

#define ODR_FILTER_BY_RECORD(SEQPTR, RECORD_TYPE, N_MAX_RECORDS) \
{                                                             \
    OverallDiagnosticRecordPtrContainer tmpSeq;               \
    size_t detectionCounter= (N_MAX_RECORDS);                 \
                                                              \
    ODR_FOREACH_RECORD_BEGIN(each, ring_.size())              \
        assert(0 != each);                                    \
        if (! each-> RECORD_TYPE .empty())                    \
        {                                                     \
            tmpSeq.push_back(each);                           \
            checkBreakCondition(detectionCounter);            \
        }                                                     \
    ODR_FOREACH_RECORD_END()                                  \
                                                              \
    SEQPTR= reverseContainer(&tmpSeq);                        \
    assert(SEQPTR->size() == tmpSeq.size());                  \
}


SeqOverallDiagnosticRecordPtr
OverallDiagnosticRecordRingBuffer::getPastCcdOverallDiagnosticRecords(
        const size_t nMaxRecords)
{
    SHARKNIR_BCU_ODR_RING_BUFFER_LOCK()
    const size_t maxNumOfRecords= checkAndAdjustNumOfRequestedRecords(
                                                              nMaxRecords);
    SeqOverallDiagnosticRecordPtr result= make_shared<
                                            SeqOverallDiagnosticRecord>();
    result->reserve(maxNumOfRecords);

    ODR_FILTER_BY_RECORD(result, ccdRecord, maxNumOfRecords)

    return result;
}


SeqOverallDiagnosticRecordPtr
OverallDiagnosticRecordRingBuffer::getPastSlopeOverallDiagnosticRecords(
            const size_t nMaxRecords)
{
    SHARKNIR_BCU_ODR_RING_BUFFER_LOCK()
    const size_t maxNumOfRecords= checkAndAdjustNumOfRequestedRecords(
                                                                nMaxRecords);
    SeqOverallDiagnosticRecordPtr result= make_shared<
                                                SeqOverallDiagnosticRecord>();
    result->reserve(maxNumOfRecords);

    ODR_FILTER_BY_RECORD(result, slopeRecord, maxNumOfRecords)

    return result;
}


SeqOverallDiagnosticRecordPtr
OverallDiagnosticRecordRingBuffer::getFutureCcdOverallDiagnosticRecords(
            const size_t nMaxRecords, const Nice::Time& timeout)
{
    SHARKNIR_BCU_ODR_RING_BUFFER_LOCK()

    waitForFutureOverallDiagnosticRecords(nCcdRecords, nMaxRecords, timeout);
    return getPastCcdOverallDiagnosticRecords(nMaxRecords);
}

SeqOverallDiagnosticRecordPtr
OverallDiagnosticRecordRingBuffer::getFutureSlopeOverallDiagnosticRecords(
            const size_t nMaxRecords, const Nice::Time& timeout)
{
    SHARKNIR_BCU_ODR_RING_BUFFER_LOCK();

    waitForFutureOverallDiagnosticRecords(nSlopeRecords, nMaxRecords, timeout);
    return getPastSlopeOverallDiagnosticRecords(nMaxRecords);
}

size_t OverallDiagnosticRecordRingBuffer::checkAndAdjustNumOfRequestedRecords(
        size_t nMaxRecords)
{
    SHARKNIR_PRECONDITION(nMaxRecords > 0,
            "User requests a positive amount of records");

    const size_t capacity= ring_.capacity();
    std::stringstream ss;
    ss << "Number of requested records is within ring buffer capacity ("
       << capacity << ")";
    SHARKNIR_PRECONDITION(nMaxRecords <= capacity, ss.str());

    if (nMaxRecords > ring_.size())
    {
        nMaxRecords= ring_.size();
    }

    return nMaxRecords;
}

void OverallDiagnosticRecordRingBuffer::checkBuffer(
        const Basda::Microgate::ByteBufferPtr& buffer)
{
    SHARKNIR_PRECONDITION(0 != buffer, "Buffer is valid");
    SHARKNIR_PRECONDITION(! buffer->empty(), "Buffer has content");
}


void OverallDiagnosticRecordRingBuffer::assignBuffer(
        Sharknir::Bcu::SeqByte& destinationRecordBuffer,
        const Basda::Microgate::ByteBufferPtr sourceBuffer,
        size_t& duplicatedReceivedRecordsCounter)
{
    if (! destinationRecordBuffer.empty())
    {
        duplicatedReceivedRecordsCounter++;
    }
    destinationRecordBuffer.assign(sourceBuffer->begin(), sourceBuffer->end());
}


void OverallDiagnosticRecordRingBuffer::detectLateFrameCounter(
        const FrameCounter frameCounter, const std::string& recordTypeStr,
        size_t *lateRecordCounter)
{
    highestReceivedFrameCounter_= std::max(frameCounter,
            highestReceivedFrameCounter_);
    if (ring_.size() == 0)
    {
        return;
    }

    FrameCounter lowerLimit= 0;
    if (delayLimitInFrameCounters_ < highestReceivedFrameCounter_)
    {
        lowerLimit= highestReceivedFrameCounter_ - static_cast<FrameCounter>(
            delayLimitInFrameCounters_);
    }

    if (lowerLimit > frameCounter)
    {
        W_LOG("Received late " << recordTypeStr <<
              " with frame counter " << frameCounter << "." <<
              "  Highest received frame counter: " <<
              highestReceivedFrameCounter_);
        (*lateRecordCounter)++;
    }
}


OverallDiagnosticRecordPtr OverallDiagnosticRecordRingBuffer::findRecordFor(
        FrameCounter frameCounter)
{
    const CircularBufferType::iterator findEnd= ring_.begin() +
            std::min(ring_.size(), delayLimitInFrameCounters_ + 1);
    CircularBufferType::iterator it= find_if(ring_.begin(), findEnd,
            FrameCounterMatch(frameCounter));
    if (findEnd == it)
    {
        OverallDiagnosticRecordPtr result(new OverallDiagnosticRecord);
        result->frameCounter= frameCounter;
        ring_.push_front(result);
        triggerAlarmsFor(nFrameCounters);
        return result;
    }
    else
    {
        return *it;
    }

    SHARKNIR_ENSURE(false, "unreachable statement");
    return OverallDiagnosticRecordPtr();
}

void OverallDiagnosticRecordRingBuffer::registerArrivalAlarm(
        const ArrivalAlarmPtr& alarm)
{
    arrivalCounters_.push_back(alarm);
}


void OverallDiagnosticRecordRingBuffer::unregisterArrivalAlarm(
        const ArrivalAlarmPtr& alarm)
{
    ArrivalAlarmContainer::iterator it=
            std::find(arrivalCounters_.begin(),
                    arrivalCounters_.end(), alarm);
    assert(arrivalCounters_.end() != it);
    arrivalCounters_.erase(it, it+1);
}

Nice::Time OverallDiagnosticRecordRingBuffer::computeTimeoutIfAny(
        const Nice::Date& endTime, const Nice::Time& timeout)
{
    const Nice::Date now= Nice::Date::now();
    if (now > endTime)
    {
        E_THROW(OverallDiagnosticRecordRingBufferException,
                "Timeout of " << timeout <<
                " occurred while waiting for records")
    }
    return endTime - now;
}

SeqOverallDiagnosticRecordPtr
OverallDiagnosticRecordRingBuffer::reverseContainer(
        OverallDiagnosticRecordPtrContainer *sharedSeq)
{
    assert(sharedSeq);

    SeqOverallDiagnosticRecordPtr records(new SeqOverallDiagnosticRecord);
    records->reserve(sharedSeq->size());
    for (OverallDiagnosticRecordPtrContainer::reverse_iterator it=
             sharedSeq->rbegin();
         it != sharedSeq->rend(); ++it)
    {
        records->push_back(**it);
    }

    return records;
}

}
}
