#ifndef _MOTION_NODES_H_
#define _MOTION_NODES_H_

#ifdef TGT_HAS_ARMS

//************

#include "Behaviors/StateNode.h"
#include "Behaviors/Nodes/ArmNode.h"
#include "Behaviors/Nodes/HeadPointerNode.h"
#include "Behaviors/Nodes/DynamicMotionSequenceNode.h"
#include "Behaviors/Nodes/PostureNode.h"
#include "Behaviors/Nodes/SoundNode.h"
#include "Behaviors/Nodes/SpeechNode.h"

#if defined(TGT_HAS_LEGS) || defined(TGT_HAS_WHEELS)
#include "Behaviors/Nodes/WalkNode.h"
#endif

#include "DualCoding/DualCoding.h"
#include "DualCoding/VisualRoutinesStateNode.h"

#include "Behaviors/Transition.h"
#include "Behaviors/Transitions/CompareTrans.h"
#include "Behaviors/Transitions/CompletionTrans.h"
#include "Behaviors/Transitions/EventTrans.h"
#include "Behaviors/Transitions/NullTrans.h"
#include "Behaviors/Transitions/RandomTrans.h"
#include "Behaviors/Transitions/SignalTrans.h"
#include "Behaviors/Transitions/SmoothCompareTrans.h"
#include "Behaviors/Transitions/TextMsgTrans.h"
#include "Behaviors/Transitions/TimeOutTrans.h"
#include "Behaviors/Transitions/VisualTargetTrans.h"
#include <Behaviors/Transitions/TimeOutTrans.h>

#ifdef TARGET_HAS_IR
#include "Behaviors/Transitions/VisualTargetCloseTrans.h"
#endif


//************

#include <Motion/MotionManager.h>

#include <IPC/DriverMessaging.h>
#include <Motion/PlanarThreeLinkArm.h>
#include <Shared/fmatCore.h>

#include <string>
using namespace std;

typedef fmat::Row<RobotInfo::NumArmJoints> ArmConfiguration;

/*
  Nodes Defined:

  SetWrist
  SetArm
  SetJoint
  SetGround
  Twist
  Sway
  Rock
  Turn
  WalkSideways
  WalkForwards
*/

extern MotionManager::MC_ID motionNodesWalkMC;

#if defined(TGT_HAS_LEGS) || defined(TGT_HAS_WHEELS)

#shortnodeclass ResetInitialPlacementXX : WalkNode : doStart
setMC(motionNodesWalkMC);
motman->setPriority(motionNodesWalkMC, priority);
getMC()->start();
postStateCompletion();

#endif

$nodeclass SetWrist(float pitch=0, float roll=0, float grip=0): ArmNode {
  virtual void doStart() {
    if(RobotInfo::NumArmJoints <= 3) {
      postStateCompletion();
      return;
    }
    cout<<"Setting wrist to ("<<pitch<<", "<<roll<<", "<<grip<<")"<<endl;
    //		getMC()->takeSnapshot();
    getMC()->setWrist(pitch, roll, grip);
  }
}

$nodeclass SetArm(float shoulder=0, float elbow=0, float wrist=0): ArmNode {
  void move(float s, float e, float w) {
    cout<<"Setting arm to ("<<s<<", "<<e<<", "<<w<<")"<<endl;
    //		getMC()->takeSnapshot();
    getMC()->setJoints(s, e, w);
  }

  virtual void doStart() {
    if(const DataEvent<ArmConfiguration>* dev = dynamic_cast<const DataEvent<ArmConfiguration>*>(event)) {
      ArmConfiguration value = dev->getData();
      move(value[0], value[1], value[2]);
    } else {
      move(shoulder, elbow, wrist);
    }
  }
}

$nodeclass SetJoint(string jointName, float val = 0, float speed = 0.4): ArmNode {
  void moveJoint(float value) {
    cout<<"Setting "<<jointName<<" to "<<val<<endl;
    unsigned int offset = 10;
    if(jointName == "ARM:shldr")
      offset = 0;
    else if(jointName == "ARM:elbow")
      offset = 1;
    else if(jointName == "ARM:wristYaw")
      offset = 2;
    else if(RobotInfo::NumArmJoints > 3) {
      if(jointName == "ARM:wristPitch")
	offset = 3;
      else if(jointName == "ARM:wristRoll" || jointName == "ARM:gripperLeft")
	offset = 4;
      else if(jointName == "ARM:gripper" || jointName == "ARM:gripperRight")
	offset = 5;
      else {
	postStateCompletion();
	return;
      }
    }
    else {
      postStateCompletion();
      return;
    }
    cout<<jointName<<" moving"<<endl;
    getMC()->takeSnapshot();
    getMC()->setMaxSpeed(offset, speed); 	 
    getMC()->setJointValue(offset, val);
  } 
	
  virtual void doStart() {
    if(const DataEvent<float>* dev = dynamic_cast<const DataEvent<float>*>(event)) {
      float value = dev->getData();
      moveJoint(value);
    } else {
      moveJoint(val);
    }
  }
}

$nodeclass SetServoPower(string jointName, bool val) : StateNode : doStart {
  /*	using namespace DriverMessaging;
    if(hasListener(ToggleServoPower::NAME)) {
    ToggleServoPower msg;
    msg.jointName = jointName;
    msg.holdPower = true;
    postMessage(msg);
    }
  */
}

#if TGT_HAS_LEGS

$nodeclass SetGround(float groundOffset = -10): WalkNode {
  void setID() {
    if(motionNodesWalkMC == MotionManager::invalid_MC_ID){
      postStateCompletion();
      return;
    }
    setMC(motionNodesWalkMC);
    motman->setPriority(motionNodesWalkMC, priority);
  }
	
  void move(float offset) {
    cout<<"Setting ground plane to "<<offset<<endl;
    getMC()->groundPlane[3] = offset;
  }

  virtual void doStart() {
    setID();
    if(const DataEvent<float>* dev = dynamic_cast<const DataEvent<float>*>(event)) {
      float off = dev->getData();
      move(off);
    } else {
      move(groundOffset);
    }
  }
}

$nodeclass Twist(float angle = 0): WalkNode {
  void setID() {
    if(motionNodesWalkMC == MotionManager::invalid_MC_ID){
      postStateCompletion();
      return;
    }
    setMC(motionNodesWalkMC);
    motman->setPriority(motionNodesWalkMC, priority);
  }
	
  void move(float howmuch) {
    cout<<"Twisting to "<<howmuch<<endl;
    getMC()->offsetA = howmuch;
  }

  virtual void doStart() {
    setID();
    if(const DataEvent<float>* dev = dynamic_cast<const DataEvent<float>*>(event)) {
      float off = dev->getData();
      move(off);
    } else {
      move(angle);
    }
  }
}

$nodeclass Sway(float offset = 0): WalkNode {
  void setID() {
    if(motionNodesWalkMC == MotionManager::invalid_MC_ID){
      postStateCompletion();
      return;
    }
    setMC(motionNodesWalkMC);
    motman->setPriority(motionNodesWalkMC, priority);
  }
	
  void move(float howmuch) {
    cout<<"Swaying to "<<howmuch<<endl;
    getMC()->offsetY = howmuch;
  }

  virtual void doStart() {
    setID();
    if(const DataEvent<float>* dev = dynamic_cast<const DataEvent<float>*>(event)) {
      float off = dev->getData();
      move(off);
    } else {
      move(offset);
    }
  }
}

$nodeclass Rock(float offset = 0): WalkNode {
  void setID() {
    if(motionNodesWalkMC == MotionManager::invalid_MC_ID){
      postStateCompletion();
      return;
    }
    setMC(motionNodesWalkMC);
    motman->setPriority(motionNodesWalkMC, priority);
  }
	
  void move(float howmuch) {
    cout<<"Rocking to "<<howmuch<<endl;
    getMC()->offsetX = howmuch;
  }

  virtual void doStart() {
    setID();
    if(const DataEvent<float>* dev = dynamic_cast<const DataEvent<float>*>(event)) {
      float off = dev->getData();
      move(off);
    } else {
      move(offset);
    }
  }
}

$nodeclass Turn(float myangle = 0): WalkNode {
  void setID() {
    if(motionNodesWalkMC == MotionManager::invalid_MC_ID){
      postStateCompletion();
      return;
    }
    setMC(motionNodesWalkMC);
    motman->setPriority(motionNodesWalkMC, priority);
  }
	
  void move(float ang) {
    cout<<"Turning by "<<ang<<endl;
    getMC()->setTargetVelocity(0, 0, 3*M_PI/180);
    getMC()->setTargetDisplacement(0,0, ang);
  }

  virtual void doStart() {
    setID();
    if(const DataEvent<float>* dev = dynamic_cast<const DataEvent<float>*>(event)) {
      float val = dev->getData();
      move(val);
    } else {
      move(myangle);
    }
  }
}

$nodeclass WalkSideways(float distance = 0) : WalkNode {
  void setID() {
    if(motionNodesWalkMC == MotionManager::invalid_MC_ID){
      postStateCompletion();
      return;
    }
    setMC(motionNodesWalkMC);
    motman->setPriority(motionNodesWalkMC, priority);
  }
	
  void move(float dist) {
    cout<<"Walking sideways "<<dist<<endl;
    getMC()->setTargetVelocity(0, 10, 0);
    getMC()->setTargetDisplacement(0, dist, 0);
  }

  virtual void doStart() {
    setID();
    if(const DataEvent<float>* dev = dynamic_cast<const DataEvent<float>*>(event)) {
      float dist = dev->getData();
      move(dist);
    } else {
      move(distance);
    }
  }
}

$nodeclass WalkForward(float distance = 0) : WalkNode {
  void setID() {
    if(motionNodesWalkMC == MotionManager::invalid_MC_ID){
      postStateCompletion();
      return;
    }
    setMC(motionNodesWalkMC);
    motman->setPriority(motionNodesWalkMC, priority);
  }
	
  void move(float dist) {
    cout<<"Walking forward by "<<dist<<endl;
    getMC()->setTargetVelocity(10, 0, 0);
    getMC()->setTargetDisplacement(dist, 0, 0);
  }

  virtual void doStart() {
    setID();
    if(const DataEvent<float>* dev = dynamic_cast<const DataEvent<float>*>(event)) {
      float dist = dev->getData();
      move(dist);
    } else {
      move(distance);
    }
  }
}

#else	//TGT_HAS_LEGS

$nodeclass SetGround(float a=-10) : StateNode : doStart {
  postStateCompletion();
}
	
$nodeclass Twist(float a=0) : StateNode : doStart {
  postStateCompletion();
}
	
$nodeclass Sway(float a=0) : StateNode : doStart {
  postStateCompletion();
}
	
$nodeclass Rock(float a=0) : StateNode : doStart {
  postStateCompletion();
}
	
$nodeclass Turn(float a=0) : StateNode : doStart {
  postStateCompletion();
}
	
$nodeclass WalkSideways(float a=0) : StateNode : doStart {
  postStateCompletion();
}
	
$nodeclass WalkForwards(float a=0) : StateNode : doStart {
  postStateCompletion();
}
	
#endif //TGT_HAS_LEGS

#endif	//TGT_HAS_ARMS

#endif