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

#include "Behaviors/StateMachine.h"
#include "DualCoding/DualCoding.h"
#include "Sound/SoundManager.h"
#include "Events/EventRouter.h"
#include "Shared/fmat.h"

#include "ChuPosture.h"

/* This class retracts the arm and provide a push
 */
class ChuArmPush : public StateNode {
public:
  ChuArmPush() : StateNode("ChuArmPush") {}


  virtual void DoStart() {
    StateNode::DoStart();

    SharedObject<MediumMotionSequenceMC> mseq_mc;

    mseq_mc->advanceTime(500);    // move the arm to the initial position (240.5,-0.1,41) in
    mseq_mc->setOutputCmd(ArmOffset, 0.6475);
    mseq_mc->setOutputCmd(ArmElbowOffset, -2.1215);
    mseq_mc->setOutputCmd(WristOffset, 1.6097);

    ////Acquire current position from the arm
    //fmat::Transform gripper = kine->linkToBase(GripperFrameOffset);
    //fmat::Column<3> gripperPos = gripper.translation();

    //fmat::Column<3> zero3D = ;


    /******************************
     * NEED DEBUG !!!!! not working
     ******************************
    */
    //Try to do inverse kinematics to find where to kick (not straight kick though)
    PostureEngine hitPosture;
    float finalX = 296.1f;
    float finalY = 8.8f;
    int numSampling = 10;
    int totalTime = 500;

//    bool reachable = hitPosture.solveLink(fmat::pack(,41.0f),fmat::QUAT_IDENTITY,GripperFrameOffset,fmat::pack(0.f, 0.f, 0.f),fmat::QUAT_IDENTITY);
    bool reachable = hitPosture.solveLinkPosition(fmat::pack(,41.0f),GripperFrameOffset,fmat::pack(0.f, 0.f, 0.f));
    

    if (reachable) {
      std::cout << "Hitting the target :-) ..." << std::endl;
      mseq_mc->advanceTime(500);
      mseq_mc->setPose(hitPosture); 
    } else {
      std::cout << "Hit target is not reachable" << std::endl;
    }
    

    /* 
    // temporary solution
    mseq_mc->advanceTime(500);    // move the arm to the final position (332.5  11.5  41.0) in
    mseq_mc->setOutputCmd(ArmOffset, 0.2124);
    mseq_mc->setOutputCmd(ArmElbowOffset, -0.6731);
    mseq_mc->setOutputCmd(WristOffset, 0.5809);
     */



    motman->addPrunableMotion(mseq_mc);
    DoStop();
  }



  virtual void processEvent(const EventBase &) {
		postStateCompletion();
	}

};

#endif