#ifndef MY_HEADERS
#define MY_HEADERS
#include "headers.h"
#endif

#ifndef MY_CONSTANTS
#define MY_CONSTANTS
#include "constants.h"
#endif

void assign_SurvivePose(SurvivePose *pose, float px, float py, float pz, float rx, float ry, float rz, float rw);

SurvivePose average_survive_poses(SurvivePose poses[BUFFER_SIZE]);

SurvivePose add_poses(SurvivePose pose1, SurvivePose pose2);

SurvivePose get_change_pose(SurvivePose original_pose, SurvivePose end_pose);

void scale_pose(SurvivePose *pose, float m);

bool snap_pose(SurvivePose *pose);

float add_to_buffer(SurvivePose pose);

class PoseQueue
{
public:
    PoseQueue(size_t max_size);

    bool push(const SurvivePose &pose);
    bool pop(SurvivePose &pose);
    size_t size() const;
    bool empty();

private:
    std::queue<SurvivePose> queue_;
    size_t max_size_;
};