#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 {
        cout << "Here" << endl;
	$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 TurnTo : PilotNode(PilotTypes::walk) : doStart {
    $reference GraspRequest::to_grasp;
    double theta1 = atan((to_grasp->getCentroid().coordY()-15)/(to_grasp->getCentroid().coordX()-15));
    double z = sqrt(pow((to_grasp->getCentroid().coordX()-15),2) + pow((to_grasp->getCentroid().coordY()-15),2));
    double theta2 = atan(120.0/z);
    pilotreq.da = theta1 + theta2;
  }

  $nodeclass WalkTo : PilotNode(PilotTypes::walk) : doStart {
    $reference GraspRequest::to_grasp;
    double z = sqrt(pow((to_grasp->getCentroid().coordX()-15),2) + pow((to_grasp->getCentroid().coordY()-15),2));
    pilotreq.dx = z;
  }




  $setupmachine {
        PlaceholdShape =C=> PlaceholdShape2 =C=> takePic


        takePic : TakeAPictureLocal
	takePic =S=> TurnTo =C=> WalkTo =C=> TurnAndCheck
	takePic =F=> SpeechNode("Failed to find shape")

	graspPlaceholder : StateNode
	
  }
}
REGISTER_BEHAVIOR(GraspRequest);