#include "cyber/visualslam.hpp"

visualslam vs;

void init()
{
    vs.cloud_create("mycloud", 3.0f);
    vs.wave_create("mywave", 1000, 0.1, {0.2f, 0.8f, 0.2f});
    vs.pose_displayer_create("aircraft");
}
int main()
{
    vs.set_init(init);
    while (1)
    {
        if (vs.ok())
        {
            float x = ((float)std::rand() / (float)RAND_MAX);
            float y = ((float)std::rand() / (float)RAND_MAX);
            float z = ((float)std::rand() / (float)RAND_MAX);
            vs.cloud_push_one({x * 10, y * 10, z * 10}, {0.8, 0.2, 0.2});
            // vs.wave_push({x});
            // vs.pose_displayer_set_left_hand(0.8, x, y, z);
            std::this_thread::sleep_for(std::chrono::microseconds(10));
        }
    }
    return 0;
}