#include "Shared/RobotInfo.h"

#ifdef TGT_CALLIOPE5KP

#include "Behaviors/StateMachine.h"

$nodeclass GraspBrick : VisualRoutinesStateNode {

  $provide PostureEngine myEngine;
  $provide fmat::Column<3> target;

  $nodeclass LookForBlob : MapBuilderNode(MapBuilderRequest::localMap) : doStart {
    NEW_SHAPE(gazeTarget, PointData, new PointData(localShS, Point(300,0,0,egocentric)));
    mapreq.searchArea = gazeTarget;
    mapreq.addObjectColor(blobDataType, "blue");
    mapreq.addMinBlobArea("blue", 400);
  }

  $nodeclass CheckBlob : VisualRoutinesStateNode : doStart {
    NEW_SHAPEVEC(blobs, BlobData, select_type<BlobData>(localShS));
    if ( blobs.size() == 0 ) {
      postStateFailure();
      return;
    }
    $reference GraspBrick::target;
		target = blobs[0]->getCentroid().getCoords();
    target[2] = 150;
    postStateSuccess();
	}

  $nodeclass MoveArm : PostureNode : doStart {
    $reference GraspBrick::target;
    // fmat::Quaternion oriTgt = fmat::Quaternion::aboutZ(-M_PI/2);
    fmat::Quaternion oriTgt = fmat::Quaternion::aboutY(-M_PI/2);
    fmat::Column<3> posOffset = fmat::ZERO3;
    fmat::Quaternion oriOffset = fmat::Quaternion::identity();
    cout << "ArmElbow max speed=" << getMC()->getMaxSpeed(ArmElbowOffset) << endl;
    cout << "Target is at " << target << endl;
    $reference GraspBrick::myEngine;
    //bool success = getMC()->solveLink(target, oriTgt, GripperFrameOffset, posOffset, oriOffset);
    bool success = myEngine.solveLink(target, oriTgt, GripperFrameOffset, posOffset, oriOffset);
    cout << "success=" << success << endl;
		if ( ! success ) {
			postStateSuccess();
			return;
		}
    getMC()->setOverlay(myEngine);
    for (unsigned int j = 0; j < NumArmJoints; j++ )
      getMC()->setMaxSpeed(ArmOffset+j, 0.2);
  }

  $nodeclass ReportPosition : StateNode : doStart {
    fmat::Column<3> pos = kine->linkToBase(GripperFrameOffset).translation();
    cout << "Position = " << pos << endl;

    $reference GraspBrick::myEngine;
    cout << "Command: ";
    for (unsigned int j = 0; j < NumArmJoints; j++ )
      cout << myEngine(ArmOffset+j).value << " ";
    cout<<endl;

    cout << "Actual : ";
    for (unsigned int j = 0; j < NumArmJoints; j++ )
      cout << state->outputs[ArmOffset+j] << " ";
    cout<<endl;
  }

  $nodeclass LowerArm : PostureNode : doStart {
    for (unsigned int j = 0; j < NumArmJoints; j++ )
      getMC()->setMaxSpeed(ArmOffset+j, 0.2);
    $reference GraspBrick::target;
    target[2] = 80;
    fmat::Quaternion oriTgt = fmat::Quaternion::aboutZ(-M_PI/2);
    //fmat::Quaternion oriTgt = fmat::Quaternion::aboutY(-M_PI/2);
    fmat::Column<3> posOffset = fmat::ZERO3;
    fmat::Quaternion oriOffset = fmat::Quaternion::identity();
		getMC()->solveLink(target, oriTgt, GripperFrameOffset, posOffset, oriOffset);
  }

  $nodeclass CloseGripper : PostureNode : doStart {
    getMC()->setOutputCmd(LeftFingerOffset, 0.2);
    getMC()->setOutputCmd(RightFingerOffset, -0.2);
  }

  $nodeclass RaiseArm : PostureNode : doStart {
    for (unsigned int j = 0; j < NumArmJoints; j++ )
      getMC()->setMaxSpeed(ArmOffset+j, 0.2);
    fmat::Column<3> target = fmat::pack(150, 0, 500);
    getMC()->solveLinkPosition(target, GripperFrameOffset, fmat::ZERO3);
  }

  $nodeclass ParkArm : PostureNode : doStart {
    for (unsigned int j = 0; j < NumArmJoints; j++ )
      getMC()->setMaxSpeed(ArmOffset+j, 0.2);
		getMC()->setOutputCmd(ArmBaseOffset, -1);
		getMC()->setOutputCmd(ArmShoulderOffset, 2.6);
		getMC()->setOutputCmd(ArmElbowOffset, -1.15);
		getMC()->setOutputCmd(ArmWristOffset, -2.15);
  }


  $nodeclass DropObject : PostureNode : doStart {
    getMC()->setOutputCmd(LeftFingerOffset, -0.5);
    getMC()->setOutputCmd(RightFingerOffset, 0.5);
  }

  $setupmachine{
    startdemo: LookForBlob =C=> check

  	check: CheckBlob
    check =S=> move
    check =F=> SpeechNode("No bar") =C=> startdemo

    move: MoveArm
  	move =F=> SpeechNode("Out of reach") =C=> startdemo
    move =C=> StateNode =T(200)=> // add a little time for the arm to settle
      ReportPosition =N=> StateNode =TM=> grab

    grab: CloseGripper =C=> ResetGripper =C=> raise

  	raise: RaiseArm =C=> StateNode =T(2000)=> drop

		drop: DropObject =C=> ParkArm =C=> startdemo
  }

}

REGISTER_BEHAVIOR(GraspBrick);

#endif