// OrbbecTest.cpp : Defines the entry point for the console application.
//

#include "stdafx.h"

#include <astra\astra.hpp>
#include <cstdio>
#include <iostream>


std::string jointNames[]
{
    "Head",
    "ShoulderSpine",
    "LeftShoulder",
    "LeftElbow",
    "LeftHand",
    "RightShoulder",
    "RightElbow",
    "RightHand",
    "MidSpine",
    "BaseSpine",
    "LeftHip",
    "LeftKnee",
    "LeftFoot",
    "RightHip",
    "RightKnee",
    "RightFoot",
    "LeftWrist",
    "RightWrist",
    "Neck"
};


int main(int argc, char** argv)
{
    astra::initialize();
    std::cout << "Astra initialized." << std::endl;

    astra::StreamSet streamSet;
    astra::StreamReader reader = streamSet.create_reader();
    std::cout << "Created stream reader." << std::endl;

    reader.stream<astra::BodyStream>().start();
    std::cout << "Started body stream reader." << std::endl;

    const int maxFramesToProcess = 500;
    int count = 0;

    do {
        std::cout << "Getting frame" << std::endl;
        astra::Frame frame = reader.get_latest_frame();
        std::cout << "Received frame." << std::endl;
        const auto bodyFrame = frame.get<astra::BodyFrame>();

        const int frameIndex = bodyFrame.frame_index();
        astra::BodyList bodies = bodyFrame.bodies();
        std::cout << "Tracked " + bodies.size() << std::endl;

        for (int i = 0; i < bodies.size(); i++)
        {

            const astra::Body *bodyArray = bodyFrame.bodies().data();
            astra::Body currentBody = bodyArray[i];
            astra::JointList jointList = currentBody.joints();
            const astra::Joint *jr = jointList.data();
            int numOfJoints = jointList.size();
            for (int j = 0; j < numOfJoints; j++)
            {
                astra_vector3f_t position = jr[j].world_position();
                std::cout << "Person " << i << std::endl
                    << "\tJoint " << jointNames[(int)jr[j].type()]
                    << ": X = " << position.x
                    << "; Y = " << position.y
                    << "; Z = " << position.z
                    << std::endl;
            }
        }

        count++;
    } while (count < maxFramesToProcess);

    std::cout << "Press any key to continue...";
    std::cin.get();

    astra::terminate();

    std::cout << "hit enter to exit program" << std::endl;
    std::cin.get();

    return 0;
}