/** @brief Skeleton Demo Behavior
 *				Shows the Skeleton tracking
 *  @author Serene H
 */
#include "Behaviors/StateMachine.h"
#include "Events/SkeletonEvent.h"
#include "Motion/MotionPtr.h"
#include "Shared/RobotInfo.h"

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

$nodeclass SkeletonDemoBehavior : VisualRoutinesStateNode {

//$nodeclass SkeletonDemoBehavior : StateNode {
//	$nodeclass ArmMirror : PostureNode : doStart {
//		fmat::Column<3> targetCam = fmat::pack(0,0,100);
//		fmat::Column<3> targetBase = kine->linkToBase(CameraFrameOffset) * targetCam;
//		fmat::Column<3> noOffset = fmat::pack(0,0,0);
//		getMC()->solveLinkPosition(targetBase, LINK, noOffset);
//	}

//	$nodeclass SkeletonListener : 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;
		}


//	virtual void setup() {
//		MotionManager::MC_ID posturemc = addMotion(MotionPtr<PostureMC>());
//		$statemachine{
//			startNode: StateNode =C=> {movenode, SkeletonListener}
//			movenode: ArmMirror[setMC(posturemc)] =E(sensorEGID)=> movenode
//		}
//	}*/

}
REGISTER_BEHAVIOR(SkeletonDemoBehavior);