//-*-c++-*-
#ifndef INCLUDED_Leapers_t__h_
#define INCLUDED_Leapers_t__h_

#ifdef TGT_IS_CHIARA

#include <iostream>

#include "Behaviors/StateMachine.h"
#include "Motion/MotionManager.h"
#include "Motion/XWalkMC.h"
#include "Shared/RobotInfo.h"
#include "Motion/PlanarThreeLinkArm.h"

using namespace std;

struct ArmCoord {
	Point pt;
	float ori;
	
	ArmCoord(): pt(), ori(0)
	{}
};

DATAEVENT_IMPLEMENTATION(Point,void*);
DATAEVENT_IMPLEMENTATION(ArmCoord,void*);

extern MotionManager::MC_ID leapWalker;

class LeapMachine : public VisualRoutinesStateNode {
 SharedObject<XWalkMC> leapWalkerMC;
 public:
  LeapMachine(const std::string &nodename="LeapMachine") : VisualRoutinesStateNode(nodename), leapWalkerMC() {}
  virtual void DoStart() {
    leapWalker = motman->addPersistentMotion(leapWalkerMC);
  }
  virtual void DoStop() {
	motman->removeMotion(leapWalker);
	leapWalker = MotionManager::invalid_MC_ID;
  }
};
// LED Commands

#shortnodeclass RedLEDOn : LedNode : constructor
    getMC()->set(RobotInfo::RedLEDMask,1);

#shortnodeclass RedLEDOff : LedNode : constructor
    getMC()->set(RobotInfo::RedLEDMask,0);

#shortnodeclass GreenLEDOn : LedNode : constructor
    getMC()->set(RobotInfo::GreenLEDMask,1);
    
#shortnodeclass GreenLEDOff : LedNode : constructor
    getMC()->set(RobotInfo::GreenLEDMask,0);
    
#shortnodeclass BlueLEDOn : LedNode : constructor
    getMC()->set(RobotInfo::BlueLEDMask,1);
    
#shortnodeclass BlueLEDOff : LedNode : constructor
    getMC()->set(RobotInfo::BlueLEDMask,0);
    
#shortnodeclass YellowLEDOn : LedNode : constructor
    getMC()->set(RobotInfo::YellowLEDMask,1);
    
#shortnodeclass YellowLEDOff : LedNode : constructor
    getMC()->set(RobotInfo::YellowLEDMask,0);

// Sound

#shortnodeclass Speak(const std::string &text = "") : SpeechNode($, _text)

#shortnodeclass Play(const std::string &soundfile = "") : SoundNode($, _soundfile)

// Walking and Posture Control
//! FORWARD
#nodeclass Forward(float distance=0) : XWalkNode : DoStart
		setMC(leapWalker);
		getMC()->setTargetVelocity(10, 0, 0);
		getMC()->setTargetDisplacement(distance, 0, 0);
	#shortnodemethod DoStartEvent
		setMC(leapWalker);
		const DataEvent<float>* dev = dynamic_cast<const DataEvent<float>*>(&event);
		if(dev == NULL) {
			DoStart();
			return;
		}
		float dist = dev->getData();
		getMC()->setTargetVelocity(10, 0, 0);
		getMC()->setTargetDisplacement(dist, 0, 0);
#endnodeclass

//! SIDEWAYS
#nodeclass Sideways(float distance=0) : XWalkNode : DoStart
		setMC(leapWalker);
		getMC()->setTargetDisplacement(0, distance, 0);
	#shortnodemethod DoStartEvent
		setMC(leapWalker);
		const DataEvent<float>* dev = dynamic_cast<const DataEvent<float>*>(&event);
		if(dev == NULL) {
			DoStart();
			return;
		}
		float dist = dev->getData();
		getMC()->setTargetDisplacement(0, dist, 0);
#endnodeclass

//! TURN
#shortnodeclass Turn(float angle=0) : XWalkNode: DoStart
		setMC(leapWalker);
		getMC()->setTargetVelocity(0, 0, 3*M_PI/180);
		getMC()->setTargetDisplacement(0, 0, angle);
	#shortnodemethod DoStartEvent
		setMC(leapWalker);
		const DataEvent<float>* dev = dynamic_cast<const DataEvent<float>*>(&event);
		if(dev == NULL) {
			DoStart();
			return;
		}
		float ang = dev->getData();
		getMC()->setTargetVelocity(0, 0, 3*M_PI/180);
		getMC()->setTargetDisplacement(0, 0, ang);
#endnodeclass

#shortnodeclass Rock(float offset = 0) : XWalkNode: DoStart
		setMC(leapWalker);
		getMC()->offsetX = offset;
	#shortnodemethod DoStartEvent(const EventBase &event)
		setMC(leapWalker);
		const DataEvent<float>* dev = dynamic_cast<const DataEvent<float>*>(&event);
		if(dev == NULL) {
			DoStart();
			return;
		}
		float howmuch = dev->getData();
		getMC()->offsetX = howmuch;
#endnodeclass

#shortnodeclass Sway(float offset = 0) : XWalkNode : DoStart
		setMC(leapWalker);
		getMC()->offsetY = offset;
	#shortnodemethod DoStartEvent(const EventBase &event)
		setMC(leapWalker);
		const DataEvent<float>* dev = dynamic_cast<const DataEvent<float>*>(&event);
		if(dev == NULL) {
			DoStart();
			return;
		}
		float howmuch = dev->getData();
		getMC()->offsetY = howmuch;
#endnodeclass

#shortnodeclass Twist(float const angle) : XWalkNode : DoStart
		setMC(leapWalker);
		getMC()->offsetA = angle;
	#shortnodemethod DoStartEvent(const EventBase &event)
		setMC(leapWalker);
		const DataEvent<float>* dev = dynamic_cast<const DataEvent<float>*>(&event);
		if(dev == NULL) {
			DoStart();
			return;
		}
		float ang = dev->getData();
		getMC()->offsetA = ang;

#shortnodeclass LegsUp(float const groundOffset=60) : XWalkNode : DoStart
		setMC(leapWalker);
		getMC()->groundPlane[3] = groundOffset;
	#shortnodemethod DoStartEvent(const EventBase &event)
		setMC(leapWalker);
		const DataEvent<float>* dev = dynamic_cast<const DataEvent<float>*>(&event);
		if(dev == NULL) {
			DoStart();
			return;
		}
		float goff = dev->getData();
		getMC()->groundPlane[3] = goff;
#endnodeclass

#shortnodeclass LegsDown(float const groundOffset=-10) : XWalkNode : DoStart
		setMC(leapWalker);
		getMC()->groundPlane[3] = groundOffset;
	#shortnodemethod DoStartEvent(const EventBase &event)
		setMC(leapWalker);
		const DataEvent<float>* dev = dynamic_cast<const DataEvent<float>*>(&event);
		if(dev == NULL) {
			DoStart();
			return;
		}
		float goff = dev->getData();
		getMC()->groundPlane[3] = goff;
#endnodeclass

#shortnodeclass Pose(const std::string &filename, float speed=0.5f) : PostureNode($, _filename) : constructor
  for (unsigned int i = 0; i < NumHeadJoints; i++)
    getMC()->setMaxSpeed(HeadOffset+i, speed);
  for (unsigned int i = 0; i < NumArmJoints; i++)
    getMC()->setMaxSpeed(ArmOffset+i, speed);
  for (unsigned int i = 0; i < NumLegs; i++)
    for (unsigned int j = 0; j < JointsPerLeg; j++)
      getMC()->setMaxSpeed(LegOffset + i*JointsPerLeg + j, speed);
#ifdef TGT_CHIARA
  getMC()->setMaxSpeed(RFrRotatorOffset, speed);
#endif


// Head motion

// Note: argument "point" to LookAtPoint must not be passed by reference, due to caching
#shortnodeclass LookAtPoint(float x, float y, float z) : HeadPointerNode : DoStart
    getMC()->lookAtPoint(x, y, z);

#shortnodeclass LookStraight : HeadPointerNode : constructor
    getMC()->setJoints(0,0,0);

//! NODYES
#nodeclass NodYes(int const numTimes=1) : DynamicMotionSequenceNode($) : constructor
    getMC()->advanceTime(500);
    getMC()->setOutputCmd(RobotInfo::HeadOffset+PanOffset, 0.f);
    getMC()->setOutputCmd(RobotInfo::HeadOffset+TiltOffset, 0.f);
    for (int i=0; i < numTimes; i++) {
      getMC()->advanceTime(1000);
      getMC()->setOutputCmd(RobotInfo::HeadOffset+TiltOffset, 1.f);
      getMC()->advanceTime(1000);
      getMC()->setOutputCmd(RobotInfo::HeadOffset+TiltOffset, 0.f);
    }
#endnodeclass

//! NODNO
#nodeclass NodNo(int const numTimes=1) : DynamicMotionSequenceNode($) : constructor
    getMC()->advanceTime(500);
    getMC()->setOutputCmd(RobotInfo::HeadOffset+TiltOffset, 0.f);
    getMC()->setOutputCmd(RobotInfo::HeadOffset+PanOffset, 0.f);
    unsigned int const nodTime = 750;
    unsigned int nodLeft = nodTime/2;
    for (int i=0; i < numTimes; i++) {
      getMC()->advanceTime(nodLeft);
      nodLeft = nodTime;
      getMC()->setOutputCmd(RobotInfo::HeadOffset+PanOffset, -0.5f);
      getMC()->advanceTime(nodTime);
      getMC()->setOutputCmd(RobotInfo::HeadOffset+PanOffset, 0.5f);
    }
    getMC()->advanceTime(nodTime/2);
    getMC()->setOutputCmd(RobotInfo::HeadOffset+PanOffset, 0.f);
#endnodeclass

#shortnodeclass ReachToObject(float time = 2000) : DynamicMotionSequenceNode($) : DoStartEvent
	const DataEvent<ArmCoord> *datev = dynamic_cast<const DataEvent<ArmCoord >*>(&event);
	if ( datev != NULL ) {
		const ArmCoord ac = datev->getData();
		PlanarThreeLinkArm solver(RobotInfo::GripperFrameOffset);
		PlanarThreeLinkArm::Solutions sols = solver.invKin3Link(ac.pt.coordX(), ac.pt.coordY(), ac.ori);
		if(sols.valid == false || sols.noSols < 1)
			postStateFailure();
		getMC()->advanceTime(time);
		getMC()->setOutputCmd(RobotInfo::ArmShoulderOffset, sols.angles(0,0));
		getMC()->setOutputCmd(RobotInfo::ArmElbowOffset, sols.angles(0,1));
		getMC()->setOutputCmd(RobotInfo::WristOffset, sols.angles(0,2));
	}


#nodeclass SwingArm(int time, float shoulder, float elbow, float wrist) : DynamicMotionSequenceNode($) : constructor
		getMC()->advanceTime(time);
		getMC()->setOutputCmd(RobotInfo::ArmShoulderOffset, shoulder);
		getMC()->setOutputCmd(RobotInfo::ArmElbowOffset, elbow);
		getMC()->setOutputCmd(RobotInfo::WristOffset, wrist);
		
#endnodeclass

/*
#shortnodeclass SwingArmLeft(int time) : SwingArm($, time, 1.2, 0.3, 1.0) : DoStart
	getMC()->play();
	cout<<"Left DoStart"<<endl;

#shortnodeclass SwingArmRight(int time) : SwingArm($, time, -0.48, -0.57, -0.34) : DoStart
	getMC()->play();
	cout<<"Right DoStart "<< time << endl;
	
*/

#nodeclass SwingArmLeft(int time) : DynamicMotionSequenceNode($) : constructor
		getMC()->advanceTime(time);
		getMC()->setOutputCmd(RobotInfo::ArmShoulderOffset, 1.2);
		getMC()->setOutputCmd(RobotInfo::ArmElbowOffset, 0.3);
		getMC()->setOutputCmd(RobotInfo::WristOffset, 1.0);
#endnodeclass

#nodeclass SwingArmRight(int time) : DynamicMotionSequenceNode($) : constructor
		getMC()->advanceTime(time);
		getMC()->setOutputCmd(RobotInfo::ArmShoulderOffset, -0.48);
		getMC()->setOutputCmd(RobotInfo::ArmElbowOffset, -0.57);
		getMC()->setOutputCmd(RobotInfo::WristOffset, -0.34);
#endnodeclass


#endif  // TGT_IS_CHIARA
#endif  // Included