#include "Shared/RobotInfo.h"

#if defined(TGT_HAS_ARMS) && defined(TGT_HAS_HEAD)

#include "Behaviors/Nodes/PostureNode.h"
#include "Behaviors/Transitions/EventTrans.h"
#include "Behaviors/StateMachine.h"
#include "Motion/MotionPtr.h"

$nodeclass Lab7p3 : StateNode {

  $nodeclass ReportGripperCord : PostureNode : doStart {
      fmat::Transform Tgripper = kine->linkToBase(GripperFrameOffset);
      fmat::Column<3> Pgripper = Tgripper.translation();
      std::cout << "Gripper Position : x = " << Pgripper[0] << " y = " << Pgripper[1] << " z = " << Pgripper[2] << endl;
  }

  $nodeclass GuideRobot : WalkNode {
    float orgx;
    float orgy;
    float orgz;
    float xdis_thres;
    float ydis_thres;
    virtual void doStart() {
      orgx = 323.822; //323.822;107.418
      orgy = -82.2101; //-82.2101;-103.367
      orgz = 360.679; //537.395;360.679
      xdis_thres = 15;
      ydis_thres = 7;
      erouter->addListener(this, EventBase::sensorEGID);
    }

    virtual void doEvent() {
      fmat::Transform Tgripper = kine->linkToBase(GripperFrameOffset);
      fmat::Column<3> gripperpos = Tgripper.translation();
      float x_dis = gripperpos[0] - orgx;
      float y_dis = gripperpos[1] - orgy;
      float z_dis = gripperpos[2] - orgz;
      float vel_fb = (x_dis/xdis_thres)*100;
      float vel_lr = (y_dis/ydis_thres)*M_PI/6;
      float x_displacement = 0;
      float rad_displacement = 0;
      int bMovefb = 0;
      int bMovelr = 0;

      if (x_dis > xdis_thres) {
        x_displacement = 20;
        bMovefb = 1;
      }
      else if (x_dis < -xdis_thres) {
        x_displacement = -20;
        bMovefb = 1;
      }

      if (y_dis > ydis_thres) {
        rad_displacement = M_PI/10;
        bMovelr = 1;
      }
      else if (y_dis < -ydis_thres) {
        rad_displacement = -M_PI/10;
        bMovelr = 1;
      }

      
      if(bMovefb)
      {
         getMC()->setTargetVelocity(vel_fb, 0, 0);
      }
      else if(bMovelr)
      {
         getMC()->setTargetVelocity(0, 0, vel_lr);
      }
      else
      {
         getMC()->setTargetVelocity(0, 0, 0);
      }
    }
  }

  $nodeclass ArmMover : PostureNode : doStart {
    //set joint angle
    getMC()->setOutputCmd(ArmBaseOffset,0);
    getMC()->setOutputCmd(ArmShoulderOffset,0.85);
    getMC()->setOutputCmd(ArmElbowOffset,0.55);
    getMC()->setOutputCmd(ArmWristOffset,0.35);
    getMC()->setOutputCmd(LeftFingerOffset,0.41);
    getMC()->setOutputCmd(RightFingerOffset,-0.41);
  }
  
  virtual void setup() {
    // define a pmc for the fixed arm posture activated
    MotionPtr<PostureMC> pmc;
    // set joint max speed
    pmc->setMaxSpeed(ArmBaseOffset, M_PI/6);
    pmc->setMaxSpeed(ArmShoulderOffset, M_PI/6);
    pmc->setMaxSpeed(ArmElbowOffset, M_PI/6);
    pmc->setMaxSpeed(ArmWristOffset, M_PI/6);
    pmc->setMaxSpeed(LeftFingerOffset, M_PI/6);
    pmc->setMaxSpeed(RightFingerOffset, M_PI/6);
    // add mc
    MotionManager::MC_ID posturemc = addMotion(pmc);

    $statemachine{
      startnode: StateNode
      holdarm: ArmMover[setMC(posturemc)]
      holdarm_final: ArmMover[setMC(posturemc)]
      guiderobot: GuideRobot
      showtrans: ReportGripperCord =TM=> showtrans
      startnode =N=> holdarm =C=> {holdarm_final, showtrans, guiderobot}
    }
  }

}

REGISTER_BEHAVIOR(Lab7p3);

#endif