#include "DebugFastScheduler.h"
#include "AdvancedErrorManagement.h"
#include "ExecutionInfo.h"
#include "MemoryOperationsHelper.h"
#include "ObjectRegistryDatabase.h"

namespace MARTe {

const uint64 ALL_CPUS = 0xFFFFFFFFFFFFFFFFull;

DebugFastScheduler::DebugFastScheduler() : 
    FastScheduler(),
    debugBinder(*this, &DebugFastScheduler::Execute) 
{
    debugService = NULL_PTR(DebugService*);
}

DebugFastScheduler::~DebugFastScheduler() {
}

bool DebugFastScheduler::Initialise(StructuredDataI & data) {
    bool ret = FastScheduler::Initialise(data);
    if (ret) {
        ReferenceContainer *root = ObjectRegistryDatabase::Instance();
        Reference serviceRef = root->Find("DebugService");
        if (serviceRef.IsValid()) {
            debugService = dynamic_cast<DebugService*>(serviceRef.operator->());
        }
    }
    return ret;
}

ErrorManagement::ErrorType DebugFastScheduler::DebugSetupThreadMap() {
    ErrorManagement::ErrorType err;
    ComputeMaxNThreads();
    REPORT_ERROR(ErrorManagement::Information, "DebugFastScheduler: Max Threads=%!", maxNThreads);

    multiThreadService = new (NULL) MultiThreadService(debugBinder);
    multiThreadService->SetNumberOfPoolThreads(maxNThreads);
    err = multiThreadService->CreateThreads();
    if (err.ErrorsCleared()) {
        rtThreadInfo[0] = new RTThreadParam[maxNThreads];
        rtThreadInfo[1] = new RTThreadParam[maxNThreads];

        for (uint32 i = 0u; i < numberOfStates; i++) {
            cpuMap[i] = new uint64[maxNThreads];
            for (uint32 j = 0u; j < maxNThreads; j++) {
                cpuMap[i][j] = ALL_CPUS;
            }
        }

        if (countingSem.Create(maxNThreads)) {
            for (uint32 i = 0u; i < numberOfStates; i++) {
                uint32 nThreads = states[i].numberOfThreads;
                cpuThreadMap[i] = new uint32[nThreads];
                for (uint32 j = 0u; j < nThreads; j++) {
                    uint64 cpu = static_cast<uint64>(states[i].threads[j].cpu.GetProcessorMask());
                    CreateThreadMap(cpu, i, j);
                }
            }
        }
    }
    return err;
}

void DebugFastScheduler::CustomPrepareNextState() {
    ErrorManagement::ErrorType err;

    err = !realTimeApplicationT.IsValid();
    if (err.ErrorsCleared()) {
        uint8 nextBuffer = static_cast<uint8>(realTimeApplicationT->GetIndex());
        nextBuffer++;
        nextBuffer &= 0x1u;

        if (!initialised) {
            cpuMap = new uint64*[numberOfStates];
            cpuThreadMap = new uint32*[numberOfStates];
            err = DebugSetupThreadMap();
        }
        if (err.ErrorsCleared()) {
            for (uint32 j = 0u; j < maxNThreads; j++) {
                rtThreadInfo[nextBuffer][j].executables = NULL_PTR(ExecutableI **);
                rtThreadInfo[nextBuffer][j].numberOfExecutables = 0u;
                rtThreadInfo[nextBuffer][j].cycleTime = NULL_PTR(uint32 *);
                rtThreadInfo[nextBuffer][j].lastCycleTimeStamp = 0u;
            }

            ScheduledState *nextState = GetSchedulableStates()[nextBuffer];
            uint32 numberOfThreads = nextState->numberOfThreads;
            for (uint32 i = 0u; i < numberOfThreads; i++) {
                rtThreadInfo[nextBuffer][cpuThreadMap[nextStateIdentifier][i]].executables = nextState->threads[i].executables;
                rtThreadInfo[nextBuffer][cpuThreadMap[nextStateIdentifier][i]].numberOfExecutables = nextState->threads[i].numberOfExecutables;
                rtThreadInfo[nextBuffer][cpuThreadMap[nextStateIdentifier][i]].cycleTime = nextState->threads[i].cycleTime;
                rtThreadInfo[nextBuffer][cpuThreadMap[nextStateIdentifier][i]].lastCycleTimeStamp = 0u;
            }
        }
    }
}

ErrorManagement::ErrorType DebugFastScheduler::Execute(ExecutionInfo & information) {
    ErrorManagement::ErrorType ret;

    if (information.GetStage() == MARTe::ExecutionInfo::StartupStage) {
    }
    else if (information.GetStage() == MARTe::ExecutionInfo::MainStage) {
        uint32 threadNumber = information.GetThreadNumber();
        (void) eventSem.Wait(TTInfiniteWait);
        if (superFast == 0u) {
            (void) countingSem.WaitForAll(TTInfiniteWait);
        }

        uint32 idx = static_cast<uint32>(realTimeApplicationT->GetIndex());

        if (rtThreadInfo[idx] != NULL_PTR(RTThreadParam *)) {
            if (rtThreadInfo[idx][threadNumber].numberOfExecutables > 0u) {
                
                // EXECUTION CONTROL HOOK
                if (debugService != NULL_PTR(DebugService*)) {
                    while (debugService->IsPaused()) {
                        Sleep::MSec(1);
                    }
                }

                bool ok = ExecuteSingleCycle(rtThreadInfo[idx][threadNumber].executables, rtThreadInfo[idx][threadNumber].numberOfExecutables);
                if (!ok) {
                    if (errorMessage.IsValid()) {
                        (void)MessageI::SendMessage(errorMessage, this);
                    }
                }
                
                uint32 absTime = 0u;
                if (rtThreadInfo[idx][threadNumber].lastCycleTimeStamp != 0u) {
                    uint64 tmp = (HighResolutionTimer::Counter() - rtThreadInfo[idx][threadNumber].lastCycleTimeStamp);
                    float64 ticksToTime = (static_cast<float64>(tmp) * clockPeriod) * 1e6;
                    absTime = static_cast<uint32>(ticksToTime);
                }
                uint32 sizeToCopy = static_cast<uint32>(sizeof(uint32));
                (void)MemoryOperationsHelper::Copy(rtThreadInfo[idx][threadNumber].cycleTime, &absTime, sizeToCopy);
                rtThreadInfo[idx][threadNumber].lastCycleTimeStamp = HighResolutionTimer::Counter();
            }
            else {
                (void) unusedThreadsSem.Wait(TTInfiniteWait);
            }
        }
    }
    return ret;
}

CLASS_REGISTER(DebugFastScheduler, "1.0")

}