#include "Behaviors/StateMachine.h"

#include "Behaviors/Nodes/PostureNode.h"
#include "Behaviors/Transitions/EventTrans.h"
#include "Motion/MotionPtr.h"
#include "Shared/RobotInfo.h"





#include "Behaviors/StateMachine.h"
#include "Behaviors/Demos/Navigation/PilotDemo.h"

#define PI (3.14159265)
#define FOODX (780+300+300-390)
#define FOODY (400)
#define FOODA (PI/2)
#define HAND WristRotateOffset

$nodeclass GetSnack2: PilotDemo {
    enum outcome {gripping, loose, failure};

    $provide ShapeRoot target;
    $provide fmat::Column<3> target_pos;

    virtual void doStart() {
        //NEW_SHAPE(foo, PointData, new PointData(worldShS, Point(FOODX, FOODY, 0, allocentric)));
        //target = foo;
        GET_SHAPE(snacks, PointData, worldShS);
        target = snacks;
        target_pos = fmat::pack(0,0,0);
    }
    
    virtual void buildMap() {
        //April Tags
		NEW_SHAPE(tag1, AprilTagData,new AprilTagData(worldShS, Point(0-390+2750-294-15,0-400+30/2+1,177.8), AprilTags::TagDetection(1) ));
		NEW_SHAPE(tag3, AprilTagData,new AprilTagData(worldShS, Point(0-390+147+15,0-400+30/2+1,177.8), AprilTags::TagDetection(3) ));
		NEW_SHAPE(tag2, AprilTagData,new AprilTagData(worldShS, Point(0-390+2750/2,0-400+30/2+1,177.8), AprilTags::TagDetection(2) ));
		NEW_SHAPE(tag0, AprilTagData,new AprilTagData(worldShS, Point(0-390+2750-15-0.5,0-400+367,177.8), AprilTags::TagDetection(0) ));
		NEW_SHAPE(tag4, AprilTagData,new AprilTagData(worldShS, Point(0-390+15+0.5,0-400+367,177.8), AprilTags::TagDetection(4) ));
		NEW_SHAPE(tag12, AprilTagData,new AprilTagData(worldShS, Point(0-390+780+300/2,800-400-15,177.8), AprilTags::TagDetection(12) ));
		NEW_SHAPE(tag17, AprilTagData,new AprilTagData(worldShS, Point(0-390+780+300+600+390/2,800-400-15,177.8), AprilTags::TagDetection(17) ));
		NEW_SHAPE(tag18, AprilTagData,new AprilTagData(worldShS, Point(0-390+780+300+600/2,1700-400-15+80,177.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(990-100,-100,0)));
		snacks->setObstacle(false);
		NEW_SHAPE(cobot, PointData, new PointData(worldShS, Point(0,0,0)));
		cobot->setObstacle(false);
	}
	
	$nodeclass Loc : PilotNode(PilotTypes::localize) : doStart {
	    //particleFilter->resetFilter();
		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 Starter : PilotNode(PilotTypes::goToShape) : doStart {
        $reference GetSnack2::target;
        $reference GetSnack2::target_pos;
        GET_SHAPE(snacks, PointData, worldShS);
        target = snacks;
		pilotreq.targetShape = target;
		cout << "=======> Starter: " << snacks->getCentroid().coordX() << "," << snacks->getCentroid().coordY() << endl;
		pilotreq.collisionAction = collisionIgnore;
		target_pos = fmat::pack(FOODX, FOODY, 0);
    }
    
    $nodeclass Turner : PilotNode(PilotTypes::walk) : doStart {
        $reference GetSnack2::target_pos;
        pilotreq.collisionAction = collisionIgnore;
        pilotreq.da = atan2(target_pos[1] - theAgent->getCentroid().coordY(), target_pos[0] - theAgent->getCentroid().coordX()) - theAgent->getOrientation();
    }
    
    // visually locate the snacks
    $nodeclass FindSnacks : MapBuilderNode(MapBuilderRequest::worldMap) : doStart {
        NEW_SHAPE(food, PointData, new PointData(worldShS, Point(FOODX, FOODY, 0, allocentric)));
        mapreq.searchArea = food;

        mapreq.addObjectColor(blobDataType, "red");
        mapreq.addBlobOrientation("red",BlobData::pillar);
        mapreq.addObjectColor(blobDataType, "blue");
        mapreq.addBlobOrientation("blue",BlobData::pillar);
    }

    // drive to target
    $nodeclass DriveSetup : PilotNode(PilotTypes::goToShape) : doStart {
        $reference GetSnack2::target;
        $reference GetSnack2::target_pos;
        NEW_SHAPEVEC(cans, BlobData, select_type<BlobData>(localShS));
        target_pos = fmat::pack(cans[0]->getCentroid().coordX(), cans[0]->getCentroid().coordY(), 150);
        
        NEW_SHAPE(ready_point, PointData, new PointData(worldShS, Point(target_pos[0], target_pos[1] - 1000, 0, allocentric)));
        target = ready_point;
        
        pilotreq.targetShape = target;
		pilotreq.collisionAction = collisionIgnore;
    }
    
    $nodeclass ArmMover : PostureNode : doStart {
        $reference GetSnack2::target_pos;
        for(unsigned int off = ArmOffset; off < ArmOffset+NumArmJoints; off++)
            getMC()->setMaxSpeed(off, 0.3);
        
        fmat::Quaternion sideGrip = fmat::Quaternion::aboutZ(M_PI/2);
        fmat::Quaternion overheadGrip = fmat::Quaternion::aboutY(-M_PI/2);
        
    	cout << "========> Reaching for " << target_pos[0] << "," << target_pos[1] << "," << target_pos[2] << endl;
        bool solved = getMC()->solveLink(target_pos, overheadGrip, GripperFrameOffset,
                                         fmat::ZERO3, fmat::Quaternion::IDENTITY);
        cout << "===========>    solved = " << solved << "    <================" << endl;
    }
    
    $nodeclass ArmMover2 : PostureNode : doStart {
        $reference GetSnack2::target_pos;
        for(unsigned int off = ArmOffset; off < ArmOffset+NumArmJoints; off++)
            getMC()->setMaxSpeed(off, 0.3);

        target_pos = fmat::pack(300,-100,150);
        fmat::Quaternion sideGrip = fmat::Quaternion::aboutZ(M_PI/2);
        fmat::Quaternion overheadGrip = fmat::Quaternion::aboutY(-M_PI/2);
        bool solved = getMC()->solveLink(target_pos, overheadGrip, GripperFrameOffset,
	    			     fmat::ZERO3, fmat::Quaternion::IDENTITY);
        cout << "===========>    solved = " << solved << "    <================" << endl;
    }
    
    $nodeclass GoToCobot : PilotNode(PilotTypes::goToShape) : doStart {
        GET_SHAPE(cobot, PointData, worldShS);
		pilotreq.targetShape = cobot;
		cout << "=======> Starter: " << cobot->getCentroid().coordX() << "," << cobot->getCentroid().coordY() << endl;
		pilotreq.collisionAction = collisionIgnore;
    }
    
    $nodeclass Grab : PostureNode : doStart{
        getMC()->setOutputCmd(LeftFingerOffset, OutputCmd(state->outputs[LeftFingerOffset] + 0.1));
        getMC()->setOutputCmd(RightFingerOffset, OutputCmd(state->outputs[RightFingerOffset] - 0.1));
    }
    
    $nodeclass CheckPressure : PostureNode : doStart{
        // check pressure
    }
    

    virtual void setup() {
        // We make the posture motion command here and supply it to the
		// PostureNode via setMC() so that it only has to be registered
		// with the MotionManager once. Registering and deregistering a
		// PostureMC inside a tight sensor loop is inefficient and leads
		// to crashes.
		MotionManager::MC_ID posturemc = addMotion(MotionPtr<PostureMC>());

        $statemachine {
            startdemo: DynamicMotionSequenceNode("arm_ready.mot") 
            checkpressure: CheckPressure[setMC(posturemc)]
            grab: Grab[setMC(posturemc)] =E(sensorEGID)=> checkpressure
            checkpressure =S<outcome>(failure)=> Loc =PILOT=> FindSnacks =C=> ArmMover =C=> grab
            checkpressure =S<outcome>(loose)=> grab
            checkpressure =S<outcome>(gripping)=> PostureNode("carrying.pos") =C=> GoToCobot =PILOT=> SpeechNode("done.") =C=> PostMachineCompletion()
            startdemo =C=> Loc =PILOT=> Starter =PILOT=> Turner =PILOT=> 
                FindSnacks =C=> DriveSetup =PILOT=> Turner =PILOT=> DynamicMotionSequenceNode("ready_to_grab.mot") =C=> ArmMover =C=> grab
        }
    }

}


REGISTER_BEHAVIOR(GetSnack2);