#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 SnackBot : PilotDemo {
    $provide Point snack;

	virtual void buildMap() {
		//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 {
		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 {
		GET_SHAPE(cobot, PointData, worldShS);
		pilotreq.targetShape = cobot;
		pilotreq.collisionAction = collisionIgnore;
	}

	$nodeclass TurnToCobot : PilotNode(PilotTypes::walk) : doStart {
		pilotreq.da = (AngTwoPi(M_PI/2) - theAgent->getOrientation());
	}

	$nodeclass GoToSnacks : PilotNode(PilotTypes::goToShape) : doStart {
		GET_SHAPE(snacks, PointData, worldShS);
		pilotreq.targetShape = snacks;
		pilotreq.collisionAction = collisionIgnore;
	}

	$nodeclass TurnToSnacks : PilotNode(PilotTypes::walk) : doStart {
		pilotreq.da = (AngTwoPi(M_PI/2) - theAgent->getOrientation());
	}

	$nodeclass FindSnack : MapBuilderNode(MapBuilderRequest::localMap) : doStart {
		mapreq.pursueShapes = false;
		mapreq.addObjectColor(blobDataType, "blue"); 
		mapreq.addBlobOrientation("blue", BlobData::poster, 320); 

		NEW_SHAPE(snack, PointData, new PointData(worldShS, Point(780+300+150-390, 800-350+150, 225, allocentric)));
		mapreq.searchArea = snack;
	}
	
    $nodeclass StupidFindSnack : MapBuilderNode(MapBuilderRequest::localMap) : doStart {
		mapreq.pursueShapes = false;
		mapreq.addObjectColor(blobDataType, "red"); 
		mapreq.addBlobOrientation("red", BlobData::poster, 320); 

	}

    $nodeclass ExtractSnack : VisualRoutinesStateNode : doStart {
        $reference SnackBot::snack;
        NEW_SHAPEVEC(blobs, BlobData, select_type<BlobData>(localShS));

        if ( blobs.size() == 0 ) {
            cout << "Didn't find any snacks!" << endl;
        }

        float maxArea = 0.0;
        int maxI = 0;
        for(unsigned int i = 0; i < blobs.size(); i++) {
            if (blobs[i]->getArea() > maxArea) {
                maxArea = blobs[i]->getArea();
                maxI = i;
            }
        }


        BlobData currentSnack = blobs[maxI];
        snack = currentSnack.getCentroid();

        postStateCompletion();
    }

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


        $statemachine{
            startdemo: DynamicMotionSequenceNode("armup.mot") =C=> find
            find: Loc =TM("go to cobot")=> goToCobot
            find =TM("go to snacks")=> goToSnacks
            find =TM("find snacks")=> findSnack
            find =TM("pick up snack")=> pickUp
            pickUp: FindSnack =C=> ExtractSnack =C=> ArmMoveClose[setMC(posturemc)] =C=> open =C=> {there, close} =C(2)=> find
            findSnack: FindSnack 
            goToSnacks: GoToSnacks =C=> TurnToSnacks =C=> find
            goToCobot: GoToCobot =C=> TurnToCobot =C=> find

            there : ArmMoveThere
            open : DynamicMotionSequenceNode("opengripper.mot")
            close : DynamicMotionSequenceNode("closegripper.mot")
        }
    }

    $nodeclass ArmMoveClose : PostureNode : doStart {
        $reference SnackBot::snack;
        printf("Snack at %f %f %f\n", snack.coordX(), snack.coordY(), snack.coordZ());
        printf("Reaching for %f %f %f\n", snack.coordX(), snack.coordY(), snack.coordZ()+GRIPPER_Z_OFFSET);

        /* what will be provided */
        fmat::Column<3> target = fmat::pack(snack.coordX() - 100,snack.coordY(),snack.coordZ()+GRIPPER_Z_OFFSET);
        
        fmat::Column<3> noOffset = fmat::pack(0, 0, 0);
    //    fmat::Column<3> targetBase = kine->linkToBase(BaseFrameOffset) * noOffset;

        for(unsigned int i=ArmOffset; i<ArmOffset+NumArmJoints; ++i)
            getMC()->setMaxSpeed(i,(float)SLOW_SPEED);
        
        
        fmat::Quaternion oriTgt = fmat::Quaternion::identity();
        fmat::Quaternion oriOffset = fmat::Quaternion::identity();

        unsigned int link = GripperFrameOffset;

        getMC()->solveLink(target, oriTgt, link, noOffset, oriOffset);
    }


    $nodeclass ArmMoveThere : DynamicMotionSequenceNode : doStart {
        $reference SnackBot::snack;

        PostureEngine p;

        /* what will be provided */
        fmat::Column<3> startPos = fmat::pack(snack.coordX() - 100,snack.coordY(),snack.coordZ()+GRIPPER_Z_OFFSET);
        fmat::Column<3> endPos = fmat::pack(snack.coordX(),snack.coordY(),snack.coordZ()+GRIPPER_Z_OFFSET);

        fmat::Column<3> noOffset = fmat::pack(0, 0, 0);
        
        unsigned int link = GripperFrameOffset;
        
        fmat::Column<3> deltaPos = (endPos - startPos);
        cout << "DeltaPos: " << deltaPos << endl;

        fmat::Column<3> deltaStep = deltaPos / NUM_STEPS;
        cout << "DeltaStep: " << deltaStep << endl;
        fmat::Column<3> pos = startPos;

        for(int i = 0; i < NUM_STEPS; i++) {
            cout << "Position: " << pos << endl;

            pos += deltaStep;
            p.solveLinkPosition(pos, link, noOffset);
            getMC()->setPose(p);
            getMC()->advanceTime(200);
        }
    }
}

REGISTER_BEHAVIOR(SnackBot);