#include "Behaviors/Demos/Navigation/PilotDemo.h"
#include "Motion/MotionPtr.h"
#include "Behaviors/StateMachine.h"
#include "Shared/RobotInfo.h"
#include <stdio.h>
#include <sstream>

#define SLOW_SPEED M_PI*1/8
#define FAST_SPEED M_PI*3/4
#define NUM_STEPS 20
#define GRIPPER_Z_OFFSET 35

$nodeclass FinalLab : PilotDemo
{

  //
  //
  //
  // 	BUILD MAP CODE:
  //
  //
  //
  // other group's:
  $provide Point snack;

  // Andrew's:
  $provide fmat::Column<3> globalArmShouldPos;
  $provide int visualTestCount;


  virtual void buildMap()
  {
    cout << "in buildMap()" << endl;
    //April Tags
    NEW_SHAPE(tag1, AprilTagData,new AprilTagData(worldShS, Point(0-390+2750-294-15,0-400+30/2+1,178.8), AprilTags::TagDetection(1) ));
    NEW_SHAPE(tag3, AprilTagData,new AprilTagData(worldShS, Point(0-390+147+15,0-400+30/2+1,178.8), AprilTags::TagDetection(3) ));
    NEW_SHAPE(tag2, AprilTagData,new AprilTagData(worldShS, Point(0-390+2750/2,0-400+30/2+1,178.8), AprilTags::TagDetection(2) ));
    NEW_SHAPE(tag0, AprilTagData,new AprilTagData(worldShS, Point(0-390+2750-15-0.5,0-400+367,178.8), AprilTags::TagDetection(0) ));
    NEW_SHAPE(tag4, AprilTagData,new AprilTagData(worldShS, Point(0-390+15+0.5,0-400+367,178.8), AprilTags::TagDetection(4) ));
    NEW_SHAPE(tag12, AprilTagData,new AprilTagData(worldShS, Point(0-390+780+300/2,800-400-15,178.8), AprilTags::TagDetection(12) ));
    NEW_SHAPE(tag17, AprilTagData,new AprilTagData(worldShS, Point(0-390+780+300+600+390/2,800-400-15,178.8), AprilTags::TagDetection(17) ));
    NEW_SHAPE(tag18, AprilTagData,new AprilTagData(worldShS, Point(0-390+780+300+600/2,1700-400-15+80,178.8), AprilTags::TagDetection(18) ));

    //Maze
    std::vector<Point> E;
    E.push_back(Point(2750-390, 0-400, 0));
    E.push_back(Point(2750-390, 800-400, 0));
    E.push_back(Point(780+300+600-390, 800-400, 0));
    E.push_back(Point(780+300+600-390, 800+980-400, 0));
    E.push_back(Point(780+300-390, 800+980-400, 0));
    E.push_back(Point(780+300-390, 800-400, 0));
    E.push_back(Point(780-390, 800-400, 0));
    E.push_back(Point(780-390, 1700-400, 0));
    E.push_back(Point(0-390, 1700-400, 0));
    E.push_back(Point(0-390, 0-400, 0));
    NEW_SHAPE(worldBounds, PolygonData, new PolygonData(worldShS, E, true));
    worldBounds->setColor("blue");

    // Virtual points
    NEW_SHAPE(snacks, PointData, new PointData(worldShS, Point((0-390+780+300+600/2),0,0)));
    NEW_SHAPE(actual_snacks, PointData, new PointData(worldShS, Point((0-390+780+300+600/2) + 100,  250,0)));
    actual_snacks->setObstacle(false);
    snacks->setObstacle(false);
    NEW_SHAPE(cobot, PointData, new PointData(worldShS, Point(2000,0,0)));
    cobot->setObstacle(false);
  }

  $nodeclass Loc : PilotNode(PilotTypes::localize) : doStart
  {
    cout << "in Loc" << endl;
    if (pilotreq.landmarks.size() == 0)
      {
	cout << "Adding Landmarks" << endl;
	GET_SHAPE(tag0, AprilTagData, worldShS);
	GET_SHAPE(tag1, AprilTagData, worldShS);
	GET_SHAPE(tag2, AprilTagData, worldShS);
	GET_SHAPE(tag3, AprilTagData, worldShS);
	GET_SHAPE(tag4, AprilTagData, worldShS);
	GET_SHAPE(tag12, AprilTagData, worldShS);
	GET_SHAPE(tag17, AprilTagData, worldShS);
	GET_SHAPE(tag18, AprilTagData, worldShS);

	pilotreq.landmarks.push_back(tag0);
	pilotreq.landmarks.push_back(tag1);
	pilotreq.landmarks.push_back(tag2);
	pilotreq.landmarks.push_back(tag3);
	pilotreq.landmarks.push_back(tag4);
	pilotreq.landmarks.push_back(tag12);
	pilotreq.landmarks.push_back(tag17);
	pilotreq.landmarks.push_back(tag18);
      }

    MapBuilderRequest* req = new MapBuilderRequest(MapBuilderRequest::localMap);
    req->setAprilTagFamily(16,5);

    pilotreq.landmarkExtractor = req;
  }

  //
  //
  //
  //	MOVE TO PLACES
  //
  //
  //

  $nodeclass GoToCobot : PilotNode(PilotTypes::goToShape) : doStart
  {
    cout << "in GoToCobot" << endl;
    GET_SHAPE(cobot, PointData, worldShS);
    //		pilotreq.forwardSpeed = 70;
    pilotreq.targetShape = cobot;
    pilotreq.collisionAction = collisionIgnore;
    pilotreq.obstacleInflation = 20;
  }

  $nodeclass TurnToCobot : PilotNode(PilotTypes::walk) : doStart
  {
    cout << "in TurnToCobot" << endl;
	// pilotreq.da = (AngTwoPi(M_PI/2) - theAgent->getOrientation());
	pilotreq.da = (AngTwoPi(M_PI/2) - theAgent->getOrientation() + AngTwoPi(0.4));
    //		pilotreq.forwardSpeed = 70;
    pilotreq.obstacleInflation = 20;
  }

  $nodeclass TurnClockwise : PilotNode(PilotTypes::walk) : doStart
  {
    cout << "in TurnToCobot" << endl;
	// pilotreq.da = (AngTwoPi(M_PI/2) - theAgent->getOrientation());
	pilotreq.da = -(AngTwoPi(M_PI/2));
    //		pilotreq.forwardSpeed = 70;
    pilotreq.obstacleInflation = 20;
  }


  $nodeclass GoToSnacks : PilotNode(PilotTypes::goToShape) : doStart
  {
    cout << "in GoToSnacks" << endl;
    //		pilotreq.forwardSpeed = 70;
    GET_SHAPE(snacks, PointData, worldShS);
    pilotreq.targetShape = snacks;
    pilotreq.collisionAction = collisionIgnore;
    pilotreq.obstacleInflation = 20;
  }

  $nodeclass TurnToSnacks : PilotNode(PilotTypes::walk) : doStart
  {
    cout << "in TurnToSnacks" << endl;
    //		pilotreq.forwardSpeed = 70;
    pilotreq.da = (AngTwoPi(M_PI/2) - theAgent->getOrientation() + AngTwoPi(0.4));
    pilotreq.obstacleInflation = 20;
  }

  $nodeclass MoveToSnacks : PilotNode(PilotTypes::goToShape) : doStart
  {
    cout << "in MoveToSnacks" << endl;
    //		pilotreq.forwardSpeed = 70;
    GET_SHAPE(actual_snacks, PointData, worldShS);
    pilotreq.targetShape = actual_snacks;
    pilotreq.collisionAction = collisionIgnore;
//    pilotreq.displayObstacles = true;
    pilotreq.obstacleInflation = 20;
  }

  $nodeclass WalkBack : PilotNode(PilotTypes::walk) : doStart
  {
    cout << "in WalkBack" << endl;
    //		pilotreq.forwardSpeed = 70;
    pilotreq.dx = -100;
  }


  //
  //
  //
  //	FINAL TOTAL FINITE STATE MACHINE
  //
  //
  //

  virtual void setup() {
    MotionManager::MC_ID posturemc = addMotion(MotionPtr<PostureMC>());


    $statemachine{
      //            startdemo: DynamicMotionSequenceNode("relaxarm.mot") =C=> Loc =C=> WalkForward
    startdemo: DynamicMotionSequenceNode("relaxarm.mot") =C=> find
	//			startdemo: Loc =C=> goToSnacks
	find: Loc =TM("go to cobot")=> goToCobot
	find =TM("go to snacks")=> goToSnacks
	find =TM("pick up snack")=> pickUp
	find =TM("throw snack")=> checkBasketPosition
	find =TM("throw snack anyways")=> throwSnack
	find =TM("walk back")=> walkBack
	walkBack: WalkBack =C=> find
	pickUp: GrabStuff =C=> WalkBack =C=> WalkBack =C=> WalkBack =C=> TurnClockwise =C=> find
	//pickUp =F=> SoundNode("howl.wav") =C=> find
	//pickUp: Look =C=> findBlob
	//findBlob: FindBlob(0)
	//findBlob =S=> reportBlob
	//findBlob =F=> SoundNode("howl.wav") =C=> find
	//reportBlob: ReportBlob()
	//reportBlob =S=> continuePickUp
	//reportBlob =F=> SoundNode("howl.wav") =C=> find
	//continuePickUp: DynamicMotionSequenceNode("grabpos.mot") =C=> PutAboveFood(1) =C=> PutAboveFood(0) =C=> EndPosition =C=> DynamicMotionSequenceNode("grip_close_crackers.mot") =C=> DynamicMotionSequenceNode("relaxarm.mot") =C=> find
	//goToSnacks: GoToSnacks =C=> TurnToSnacks =C=> MoveToSnacks =C=> find
	goToSnacks: MoveToSnacks =C=> TurnToSnacks =C=> find

	goToCobot: GoToCobot =C=> TurnToCobot =C=> find
    
	checkBasketPosition: GetArmShoulderOffset =C=> FindBlobs =C=> GetTolerance =C=> visualTest
	visualTest: VisualTest
	visualTest =S=> throwSnack
	visualTest =F=> SoundNode("howl.wav") =C=> find // go and orient to starting point and orientation
			
	throwSnack: DynamicMotionSequenceNode("relax.mot") =C=> DynamicMotionSequenceNode("open.mot") =C=> StateNode =T(1000)=> PostureNode("e.pos") =C=> StateNode =T(1000)=> DynamicMotionSequenceNode("relax.mot") =C=> find
        }
  }


  //
  //
  //
  //	FIND AND PICK UP SNACKS ON FLOOR
  //
  //
  //

  $nodeclass GrabStuff : VisualRoutinesStateNode
    {

      $provide fmat::Column<3> ellipseCntr;

      $nodeclass Look() : HeadPointerNode : doStart
	{
	  cout << "in Look()" << endl;
	  getMC()->lookAtPoint(200, 0, 0);
	}


      $nodeclass FindBlob(int num) : MapBuilderNode(MapBuilderRequest::localMap) : doStart
	{
	  cout << "in FindBlob()" << endl;
	  if(num == 0)
	    mapreq.addObjectColor(blobDataType, "red");
	  else if(num == 1)
	    mapreq.addObjectColor(blobDataType, "blue");
	  else if(num == 2)
	    mapreq.addObjectColor(blobDataType, "green");
	  else
	    cout << "cannot find blob: wrong color: " << num << endl;
	  // postStateSuccess();
	}


      $nodeclass ReportBlob : VisualRoutinesStateNode : doStart
	{
	  cout << "in ReportBlob()" << endl;
	  NEW_SHAPEVEC(blobs,BlobData,select_type<BlobData>(localShS));
	  NEW_SHAPE(ellipse, BlobData, max_element(blobs, BlobData::AreaLessThan()));
	  if (ellipse.isValid())
	    {
	      $reference GrabStuff::ellipseCntr;
	      ellipseCntr = ellipse->getCentroid().getCoords();
	      cout << "found" << endl;
	      // postStateSuccess();
	    }
	  else
	    {
	      cout << " not found" << endl;
	      //postParentFailure();
	    }
		postStateCompletion();
	}

      $nodeclass PutAboveFood(int above) : DynamicMotionSequenceNode : doStart
	{
	  cout << "in PutAboveFood" << endl;
	  $reference GrabStuff::ellipseCntr;
	  cout << "Blob at: " << ellipseCntr << endl;
	  fmat::Transform gripperStart = kine->linkToBase(GripperFrameOffset);
	  fmat::Column<3> gripperStartPos = gripperStart.translation();
	  cout << "Arm at: " << gripperStartPos << endl;
	  fmat::Column<3> finalPos = ellipseCntr;
	  //finalPos[0] -= 50;
	  if(above == 1)
	    finalPos[2] = 100;
	  else if(above == 0)
	    finalPos[2] = 60;
	  else
	    finalPos[2] = 100;
	  fmat::Column<3> noOffset = fmat::pack(0, 0, 0);
	  PostureEngine movearm;
	  fmat::Quaternion oriOffset = fmat::Quaternion::identity();

	  bool fromSolveLink = movearm.solveLink(finalPos, fmat::Quaternion::aboutY(-M_PI/2), GripperFrameOffset, noOffset, oriOffset);
	  cout << "Solved link? " << fromSolveLink << endl;
	  getMC()->advanceTime(1000);
	  getMC()->setPose(movearm);
	}

      $nodeclass EndPosition : DynamicMotionSequenceNode : doStart
	{
	  cout << "in EndPosition" << endl;
	  fmat::Transform gripperEnd = kine->linkToBase(GripperFrameOffset);
	  fmat::Column<3> gripperEndPos = gripperEnd.translation();
	  cout << "gripper above object: " << gripperEndPos << endl;
	  postStateCompletion();
	}
 
      $setupmachine {
      startInsideMachine: Look =C=> findBlob
	  findBlob: FindBlob(0)
	  findBlob =C=> reportBlob
	  reportBlob: ReportBlob
	  reportBlob =C=> continuePickUp
	  continuePickUp: DynamicMotionSequenceNode("grabpos.mot") =C=> PutAboveFood(1) =C=> PutAboveFood(0) =C=> EndPosition =C=> DynamicMotionSequenceNode("grip_close_crackers.mot") =C=> DynamicMotionSequenceNode("relaxarm.mot") =C=> PostMachineCompletion
	  }

    }

  //
  //
  //
  // CHECK WHETHER IN FRONT OF BASKET
  //
  //
  //

  $nodeclass GetArmShoulderOffset : PostureNode : doStart
    {
      $reference FinalLab::globalArmShouldPos;
      fmat::Transform currArmShouldPos = kine->linkToBase(ArmShoulderOffset);
      //globalArmShouldPos.setCoords(currArmShouldPos.translation()); //need not just translation but orientation
      globalArmShouldPos = currArmShouldPos.translation();
      NEW_SHAPE(shoulderPos, PointData, new PointData(localShS, Point(globalArmShouldPos[0], globalArmShouldPos[1],globalArmShouldPos[2])));
      //postStateCompletion();
    }

  $nodeclass FindBlobs : MapBuilderNode(MapBuilderRequest::localMap) : doStart
  {
    mapreq.addObjectColor(blobDataType, "red");
    mapreq.addObjectColor(blobDataType, "blue");
    //postStateCompletion();
  }
  // do I use "(MapBuilderRequest::localMap)" to project onto local space?

  $nodeclass GetTolerance : VisualRoutinesStateNode : doStart
    {
      vector<Point> define_points;
      NEW_SHAPEVEC(blob_shapes, BlobData, select_type<BlobData>(localShS));
      NEW_SHAPEVEC(blobs, BlobData, stable_sort(blob_shapes,not2(BlobData::AreaLessThan())));
      //blobs = stable_sort(blob_shapes,not2(BlobData::AreaLessThan()));
      Point centerOne = blobs[0]->getCentroid();
      centerOne.setCoords(centerOne.coordX() - 100,centerOne.coordY(),centerOne.coordZ());
      define_points.push_back(centerOne);
      Point centerTwo = blobs[1]->getCentroid();
      centerTwo.setCoords(centerTwo.coordX() - 100,centerTwo.coordY(),centerTwo.coordZ());
      define_points.push_back(centerTwo);
      Point topPoint;
      float topPointY = ((centerOne.coordY() + centerTwo.coordY()) / 2.0);
      topPoint.setCoords(centerTwo.coordX() - 75,topPointY,centerTwo.coordZ());
      define_points.push_back(topPoint);
      Point bottomPoint;
      bottomPoint.setCoords(centerTwo.coordX() - 125,topPointY,centerTwo.coordZ());
      define_points.push_back(bottomPoint);
      //float yCenter = (centerOne.coordY() + centerTwo.coordY()) / 2.0;
      //float zCenter = (centerOne.coordZ() + centerTwo.coordZ()) / 2.0;
      //Point finalCenter(xCenter, yCenter, zCenter, egocentric);
      // so is this final point, and the other points for that matter, in local space?
      //NEW_SHAPE(tolerance, EllipseData, new EllipseData(localShS, finalCenter, 500, 500, 0));
      NEW_SHAPE(tolerance_polygon, PolygonData, new PolygonData(localShS, define_points, true, true, true));
      postStateCompletion();
    }

  $nodeclass VisualTest : VisualRoutinesStateNode : doStart
    {
    
      $reference FinalLab::visualTestCount;
      GET_SHAPE(tolerance_polygon,PolygonData,localShS);
      GET_SHAPE(shoulderPos,PointData,localShS);

      if (tolerance_polygon->isInside(shoulderPos))
	{
	  ++visualTestCount;
        }
      else
	{
	  visualTestCount = visualTestCount;
        }

      if(visualTestCount == 1)
	{
	  postStateSuccess();
	}
      else
	{
	  postStateFailure();
	}
    }

}

REGISTER_BEHAVIOR(FinalLab);