#include "Behaviors/StateMachine.h"

$nodeclass GraspRequest : VisualRoutinesStateNode {

  $provide ShapeRoot to_grasp;
  $provide ShapeRoot local_grasp;

  $nodeclass PlaceholdShape : MapBuilderNode(MapBuilderRequest::worldMap) : doStart {
    mapreq.addObjectColor(cylinderDataType, "red");
  }

  $nodeclass PlaceholdShape2 : VisualRoutinesStateNode : doStart {
    $reference GraspRequest::to_grasp;
    NEW_SHAPEVEC(shapes, CylinderData, select_type<CylinderData>(worldShS));

    to_grasp = shapes[0];
    postStateCompletion();
  }

  $nodeclass TakeAPictureLocal : MapBuilderNode(MapBuilderRequest::localMap) : doStart {
	$reference GraspRequest::to_grasp;
	$reference GraspRequest::local_grasp;
	local_grasp = ShapeRoot();

	mapreq.addAttributes(to_grasp);

	SHAPEROOTVEC_ITERATE(localShS, cur_shape) {
		if (cur_shape.getId() == to_grasp->getLastMatchId()) {
			local_grasp = cur_shape;
			break;
		}
	} END_ITERATE
		 
	if (local_grasp.isValid())
    postStateSuccess();
  else
    postStateFailure();
  }

  $nodeclass TurnAndCheck : StateNode {

	$nodeclass MoveArm : DynamicMotionSequenceNode : doStart {
		cout << "moving arm" << endl;
		PostureEngine pe;
		fmat::Column<3> init_pos = kine->getPosition(GripperFrameOffset);
		fmat::Column<3> offset = fmat::pack(0, -100, -30);
		fmat::Column<3> new_pos = init_pos + offset;

		cout << "solving for arm position" << endl;
		cout << "ArmBase = " << pe.getOutputCmd(ArmBaseOffset).value << endl;
		pe.solveLinkPosition(new_pos, GripperFrameOffset, fmat::pack(0, 0, 0));
		cout << "After moving, ArmBase = " << pe.getOutputCmd(ArmBaseOffset).value << endl;
		cout << "After moving, ArmBase weight = " << pe.getOutputCmd(ArmBaseOffset).weight << endl;

		cout << "moving arm" << endl;
		getMC()->advanceTime(500);
		getMC()->setSpeed(0.4);
		getMC()->setPose(pe);
	}

	$nodeclass FindAprilTag : MapBuilderNode(MapBuilderRequest::cameraMap) : doStart {
		mapreq.setAprilTagFamily();
	}

	$nodeclass ConfirmAprilTag : StateNode : doStart {
		NEW_SHAPEVEC(april_tags, AprilTagData, select_type<AprilTagData>(camShS));
		if (april_tags.size() == 0) {
			postStateFailure();
			return;
		}

		Point p(camSkS.getWidth()/2, camSkS.getHeight()/2);
		cout << "center of cam: " << p << endl;
		cout << "center of tag: " << april_tags[0]->getCentroid() << endl;

		if ((p - april_tags[0]->getCentroid()).xyNorm() / camSkS.getWidth() > 0.1)
			postStateFailure();
		else
			postStateSuccess();
		return;
	}
		
	$setupmachine {
		MoveArm =C=>
		LookAtGripper =C=>
		FindAprilTag =C=>
		confirm

		confirm: ConfirmAprilTag

		confirm =S=> SpeechNode("gripping shape")
		confirm =F=> SpeechNode("Failed to grip shape")
	}
  }

  $nodeclass MoveTo : PilotNode(PilotTypes::waypointWalk) : doStart {
    $reference GraspRequest::to_grasp;
    double theta1 = atan((to_grasp->getCentroid().coordY())/(to_grasp->getCentroid().coordX()));
    double z = sqrt(pow((to_grasp->getCentroid().coordX()),2) + pow((to_grasp->getCentroid().coordY()),2));

    double theta2 = 0.0;
    if (to_grasp->getCentroid().coordY() < 0) {
        theta2 = atan(120.0/z);
    }
    else {
       theta2 = atan(70.0/z);
    }

    WaypointList* wpl = new WaypointList;
    wpl->addEgocentricWaypoint(0,0,theta1+theta2,true,50.f,.25f);
    wpl->addEgocentricWaypoint(z-300,0,0,true,50.f,.25f);
    pilotreq.waypointList = wpl;
  }

  $setupmachine {
        PlaceholdShape =C=> PlaceholdShape2 =C=> takePic
        takePic : TakeAPictureLocal
  takePic =S=> PostureNode("pregrab.pos") =C=> MoveTo =C=> PostureNode("grab.pos") =C=> TurnAndCheck
  takePic =F=> SpeechNode("Failed to find shape")

  graspPlaceholder : StateNode
  
  }
}
REGISTER_BEHAVIOR(GraspRequest);