/** @brief Skeleton Demo Behavior
 *			Fetches skeleton events
 *  @author Serene H
 */
#include "Behaviors/StateMachine.h"
#include "Events/SkeletonEvent.h"
#include "Shared/RobotInfo.h"

#ifdef TGT_IS_CALLIOPE5
#define LINK LeftFingerFrameOffset
#else
#define LINK GripperFrameOffset
#endif

$nodeclass SkeletonDemoBehavior : VisualRoutinesStateNode {

	void doEvent() {
	
		const SkeletonEvent &sEv = *(const SkeletonEvent*)event;
		std::vector<DualCoding::Skeleton> skel(sEv.getSkeletons());

		std::cout << "Skeleton Event Received with " << skel.size() << " skeletons." << endl;
		{
			worldShS.deleteShapes<SkeletonData>();
			for (unsigned int i = 0 ; i < skel.size() ; i++) {
				NEW_SHAPE(skeleton, SkeletonData, new SkeletonData(worldShS, skel[i]));
			}
		}
	}

	void doStart() {
		erouter->addListener(this, EventBase::visSkelEGID);
		cout << "Skeleton Tracking Demo Started!" << endl;
	}

}
REGISTER_BEHAVIOR(SkeletonDemoBehavior);