#include "VirtualRobotReaderExampleClient.h"

#include <ArmarXCore/core/time/Metronome.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>

#include <RobotAPI/libraries/armem/core/Time.h>


namespace armarx::robot_state
{
    VirtualRobotReaderExampleClient::VirtualRobotReaderExampleClient() = default;

    armarx::PropertyDefinitionsPtr VirtualRobotReaderExampleClient::createPropertyDefinitions()
    {
        armarx::PropertyDefinitionsPtr defs =
            new ComponentPropertyDefinitions(getConfigIdentifier());

        defs->topic(debugObserver);

        defs->optional(properties.robotName, "p.robotName",
                       "The name of the robot to use.");
        defs->optional(properties.updateFrequency, "p.updateFrequency [Hz]",
                       "The frequency of the running loop.");

        return defs;
    }


    std::string VirtualRobotReaderExampleClient::getDefaultName() const
    {
        return "VirtualRobotReaderExampleClient";
    }


    void VirtualRobotReaderExampleClient::onInitComponent()
    {
    }


    void VirtualRobotReaderExampleClient::onConnectComponent()
    {
        ARMARX_IMPORTANT << "Running virtual robot synchronization example.";

        task = new SimpleRunningTask<>([this]()
        {
            this->run();
        });
        task->start();
    }


    void VirtualRobotReaderExampleClient::onDisconnectComponent()
    {
        task->stop();
    }


    void VirtualRobotReaderExampleClient::onExitComponent()
    {
    }


    void VirtualRobotReaderExampleClient::run()
    {
        Metronome metronome(Frequency::Hertz(properties.updateFrequency));
        while (task and not task->isStopped())
        {
            const armem::Time now = armem::Time::Now();

            // Initialize the robot if needed.
            if (robot == nullptr)
            {
                // The TIMING_* macros are optional, you do not need them.
                TIMING_START(getRobot);

                robot = virtualRobotReaderPlugin->get().getRobot(properties.robotName, now);
                if (robot)
                {
                    // Only print timing once the robot is loadable & loaded.
                    TIMING_END_STREAM(getRobot, ARMARX_INFO);
                }
                else
                {
                    ARMARX_WARNING << deactivateSpam(10) << "Could not create virtual robot.";
                }
            }
            if (robot)
            {
                ARMARX_INFO << deactivateSpam(60) << "Synchronizing robot.";

                TIMING_START(synchronizeRobot);
                ARMARX_CHECK(virtualRobotReaderPlugin->get().synchronizeRobot(*robot, now));
                TIMING_END_STREAM(synchronizeRobot, ARMARX_INFO);


                // Do something with the robot (your code follows here, there are just a examples) ...

                Eigen::Matrix4f globalPose = robot->getGlobalPose();
                (void) globalPose;

                std::vector<std::string> nodeNames = robot->getRobotNodeNames();
                (void) nodeNames;

                // End.
            }

            metronome.waitForNextTick();
        }
    }

}  // namespace armarx::robot_state