#include <rtt/rtt-config.h>

#ifdef OS_RT_MALLOC
// need access to all TLSF functions embedded in RTT
#define ORO_MEMORY_POOL
#include <rtt/os/tlsf/tlsf.h>
#endif // OS_RT_MALLOC
#include <rtt/os/main.h>
#include <rtt/TaskContext.hpp>
#include <rtt/transports/corba/TaskContextServer.hpp>
#include <rtt/transports/corba/TaskContextProxy.hpp>
#include <rtt/OperationCaller.hpp>
#include <rtt/Activity.hpp>

int ORO_main(int argc, char* argv[])
{

#ifdef OS_RT_MALLOC
    size_t                  memSize     = 10000;
    void*                   rtMem       = 0;
    size_t                  freeMem     = 0;

    // init rt mem pool
    if (0 < memSize)
    {
        // don't calloc() as is first thing TLSF does.
        rtMem = malloc(memSize);
        assert(0 != rtMem);
        freeMem = init_memory_pool(memSize, rtMem);
        if ((size_t)-1 == freeMem)
        {
            std::cout << "Invalid memory pool size of " << memSize
                      << " bytes (TLSF has a several kilobyte overhead)." << std::endl;
            free(rtMem);
            return -1;
        }
        std::cout << "Real-time memory: " << freeMem << " bytes free of "
              << memSize << " allocated." << std::endl;
    }
#endif // OS_RT_MALLOC

    // Corba initialization
    RTT::corba::TaskContextServer::InitOrb(argc, argv, 1.0);
    RTT::corba::TaskContextServer::ThreadOrb();

    // Create remote component
    RTT::TaskContext remoteTask("MyTask");
    remoteTask.setActivity(new RTT::Activity(5, 0.01));
    RTT::corba::TaskContextServer* taskServer = RTT::corba::TaskContextServer::Create(&remoteTask, false);

    // Create a proxy for the remote component
    RTT::TaskContext *taskProxy = RTT::corba::TaskContextProxy::Create(RTT::corba::TaskContextServer::getIOR(&remoteTask), true);

    // Create a local component with the proper OperationCaller
    RTT::TaskContext localTask("LocalTask");
    RTT::OperationCaller<double(void)> getPeriodOpCaller("getPeriodOpCaller");
    localTask.requires("localService")->addOperationCaller(getPeriodOpCaller);

    // Connect the local operation caller to the remote operation
    localTask.requires("localService")->getOperationCaller("getPeriodOpCaller")
            ->setImplementationPart(taskProxy->provides()->getOperation("getPeriod"),
                    localTask.engine());

    std::cout << "Currently used rt-mem bytes = " << get_used_size(rtMem) << std::endl;

    for(int i=0;i<20;i++)
    {
        double period;
        RTT::SendHandle<double(void)> handle = getPeriodOpCaller.send();

        RTT::SendStatus ss = handle.collect(period);

        if (ss == RTT::SendSuccess) {
            std::cout << "Period was: " << period << std::endl;
        }

        std::cout << "Invoke count = " << i << ", currently used rt-mem bytes = " << get_used_size(rtMem) << std::endl;
    }

    std::cout << "Currently used rt-mem bytes = " << get_used_size(rtMem) << std::endl;

#ifdef OS_RT_MALLOC
    if (0 != rtMem)
    {
        std::cout << "TLSF bytes allocated=" << memSize
                  << " overhead=" << (memSize - freeMem)
                  << " max-used=" << get_max_size(rtMem)
                  << " currently-used=" << get_used_size(rtMem)
                  << " still-allocated=" << (get_used_size(rtMem) - (memSize - freeMem))
                  << "\n";

        destroy_memory_pool(rtMem);
        free(rtMem);
    }
#endif // OS_RT_MALLOC

    return 0;
}