#include "Behaviors/Demos/Navigation/PilotDemo.h"
#include "Motion/MotionPtr.h"

#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 {
    $provide Point snack;
	$provide fmat::Column<3> ellipseCntr;

    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 - 50,0,0)));
        snacks->setObstacle(false);
        NEW_SHAPE(cobot, PointData, new PointData(worldShS, Point(-50,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;
    }

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

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

    }

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

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

    $nodeclass WalkForward : PilotNode(PilotTypes::walk) : doStart {
		cout << "in TurnToSnacks" << endl;
		pilotreq.forwardSpeed = 10;
        pilotreq.dx = 20;
    }



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


        $statemachine{
            startdemo: DynamicMotionSequenceNode("relaxarm.mot") =C=> Loc =C=> WalkForward
//            startdemo: DynamicMotionSequenceNode("relaxarm.mot") =C=> Loc =C=> walkForward goToSnacks
//			startdemo: Loc =C=> goToSnacks
            find: Loc =TM("go to cobot")=> goToCobot
            find =TM("go to snacks")=> goToSnacks
            find =TM("pick up snack")=> pickUp
            pickUp: Look =C=> FindEllipse =C=> ReportEllipse =S=> DynamicMotionSequenceNode("grabpos.mot") =C=> PutAboveFood =C=> DownToFood =C=> EndPosition =C=> DynamicMotionSequenceNode("grip_close_crackers.mot") =C=> DynamicMotionSequenceNode("relaxarm.mot")
            goToSnacks: GoToSnacks =C=> TurnToSnacks =C=> find
            goToCobot: GoToCobot =C=> TurnToCobot =C=> find
        }
}



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


$nodeclass FindEllipse() : MapBuilderNode(MapBuilderRequest::localMap) :
doStart{
	cout << "in FindEllipse()" << endl;
 mapreq.addObjectColor(blobDataType, "blue");
 mapreq.addObjectColor(blobDataType, "red");
}

$nodeclass ReportEllipse() : VisualRoutinesStateNode : doStart{
		cout << "in ReportEllipse()" << endl;
        NEW_SHAPEVEC(blobs,BlobData,select_type<BlobData>(localShS));
NEW_SHAPE(ellipse, BlobData, max_element(blobs, BlobData::AreaLessThan()));
 if (ellipse.isValid()) {
$reference FinalLab::ellipseCntr;
ellipseCntr = ellipse->getCentroid().getCoords();
 postStateSuccess();
} else {
postStateFailure();
 }
}

$nodeclass PutAboveFood : DynamicMotionSequenceNode : doStart{
	cout << "in PutAboveFood" << endl;
  $reference FinalLab::ellipseCntr;
          cout << "ellipse " << ellipseCntr << endl;
  fmat::Transform gripperStart =
  kine->linkToBase(GripperFrameOffset);
  fmat::Column<3> gripperStartPos = gripperStart.translation();
          cout << "initial conditions: " << gripperStartPos << ":"
                   << ( ellipseCntr[0] - gripperStartPos[0] ) << endl;
  fmat::Column<3> finalPos = ellipseCntr;
    finalPos[2] = 160;
//  fmat::Column<3> offset = fmat::pack(diff[0] / 20, 0, 0);

//  cout << "diff: " << diff << endl;
  fmat::Column<3> noOffset = fmat::pack(0, 0, 0);
  PostureEngine movearm;

          fmat::Quaternion oriTgt =
fmat::Quaternion::fromMatrix(gripperStart);
          fmat::Quaternion oriOffset = fmat::Quaternion::identity();

          cout << "offset: " << oriTgt << endl;

//          for (int i = 0; i < 20; i++) {
//  gripperStartPos = gripperStartPos + offset;
//  fmat::Quaternion aboutY = fmat::Quaternion::aboutY(-M_PI/2);
    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);
  cout << "gripper: " << gripperStartPos << endl;
//  }
}

$nodeclass DownToFood : DynamicMotionSequenceNode : doStart{
	cout << "in DownToFood" << endl;
  $reference FinalLab::ellipseCntr;
          cout << "ellipse " << ellipseCntr << endl;
  fmat::Transform gripperStart =
  kine->linkToBase(GripperFrameOffset);
  fmat::Column<3> gripperStartPos = gripperStart.translation();
          cout << "initial conditions: " << gripperStartPos << ":"
                   << ( ellipseCntr[0] - gripperStartPos[0] ) << endl;
  fmat::Column<3> finalPos = ellipseCntr;
    finalPos[2] = 80;
//  fmat::Column<3> offset = fmat::pack(diff[0] / 20, 0, 0);

//  cout << "diff: " << diff << endl;
  fmat::Column<3> noOffset = fmat::pack(0, 0, 0);
  PostureEngine movearm;

          fmat::Quaternion oriTgt =
fmat::Quaternion::fromMatrix(gripperStart);
          fmat::Quaternion oriOffset = fmat::Quaternion::identity();

          cout << "offset: " << oriTgt << endl;

//          for (int i = 0; i < 20; i++) {
//  gripperStartPos = gripperStartPos + offset;
//  fmat::Quaternion aboutY = fmat::Quaternion::aboutY(-M_PI/2);
    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);
  cout << "gripper: " << gripperStartPos << endl;
//  }
}


$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();
}

}

REGISTER_BEHAVIOR(FinalLab);