#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 MAX_RANGE 3000

#define BUF_SIZE 64

$nodeclass Executor : PilotDemo {
    $provide vector<ShapeRoot> allShapes;
    $provide ShapeRoot relativeShape;
    $provide vector<string> cmds;
    $provide vector<string> validCmds;

    /*********************************************
     * Actions
     *********************************************/
    
    $nodeclass GoTo : PilotNode(PilotTypes::goToShape) : doStart {
        $reference Executor::allShapes;

        if(allShapes.size() <= 0) {
            postStateFailure();
            return;
        }

        Point target = allShapes[0]->getCentroid();
        allShapes[0]->setObstacle(false);
        Point robot = theAgent->getCentroid();

        Point path = (target-robot);
        Point dir = path.unitVector();
        cout << "robot: " << robot << " target: " << target << endl;
        cout << "path: " << path << " dir: " << dir;
        float dist = target.distanceFrom(robot) - 300.0;
        Point destPoint = dir * dist + robot;
        cout << "destPoint: " << destPoint << endl;
        
        {
            GET_SHAPE(dest, PointData, worldShS);
            if ( dest.isValid() )
                worldShS.deleteShape(dest);
        }
        NEW_SHAPE(dest, PointData, new PointData(worldShS, destPoint));
        dest->setObstacle(false);

        pilotreq.targetShape = dest;
        pilotreq.collisionAction = collisionIgnore;
    }
    
    $nodeclass LookAt : HeadPointerNode : doStart {
        $reference Executor::allShapes;

        if(allShapes.size() <= 0) {
            postStateFailure();
            return;
        }

        Point target = VRmixin::mapBuilder->importWorldToLocal(allShapes[0])->getCentroid();

        getMC()->lookAtPoint(target.coordX(), target.coordY(), target.coordZ());

    }

    $nodeclass TurnTo : PilotNode(PilotTypes::walk) : doStart {
        $reference Executor::allShapes;

        if(allShapes.size() <= 0) {
            postStateFailure();
            return;
        }

        Point target = allShapes[0]->getCentroid();
        Point robot = theAgent->getCentroid();

        Point path = (target-robot);
        AngSignPi dir = path.atanYX();
        AngSignPi turn = dir - theAgent->getOrientation();
        cout << "robot: " << robot << " target: " << target << endl;
        cout << "dir: " << dir << " turn: " << turn;

        pilotreq.dx = 0;
        pilotreq.dy = 0;
        pilotreq.da = turn; 
        pilotreq.collisionAction = collisionIgnore;
     }
      
    $nodeclass GrabNode(int above) : DynamicMotionSequenceNode : doStart
	{
        $reference Executor::allShapes;

        if(allShapes.size() <= 0) {
            postStateFailure();
            return;
        }

        Point target = VRmixin::mapBuilder->importWorldToLocal(allShapes[0])->getCentroid();

	  fmat::Transform gripperStart = kine->linkToBase(GripperFrameOffset);
	  fmat::Column<3> gripperStartPos = gripperStart.translation();
	  cout << "Arm at: " << gripperStartPos << endl;
	  fmat::Column<3> finalPos;
      finalPos[0] = target.coordX();
      finalPos[1] = target.coordY();
      finalPos[2] = target.coordZ();
	  //finalPos[0] -= 50;

      if(above == 1)
        finalPos[2] = 150;
      else if(above == 0)
        finalPos[2] = 80;
      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);
	}
    
    /*********************************************
     * Get Shapes
     *********************************************/

    $nodeclass Remember : MapBuilderNode(MapBuilderRequest::worldMap) : doStart
    {
        $reference Executor::allShapes;
        $reference Executor::relativeShape;

        if(allShapes.size() < 1) {
            printf("No shapes in current set\n");
        } else if (allShapes.size() > 1) {
            printf("More than 1 shape in current set, please filter\n");
        } else {
            relativeShape = allShapes[0];
            printf("Shape remembered\n");
        }
    }

    $nodeclass StoreObjects : MapBuilderNode(MapBuilderRequest::worldMap) : doStart
    {
        $reference Executor::allShapes;

        allShapes.clear();
        IsType linetest(lineDataType);
        IsType ellipsetest(ellipseDataType);
        vector<ShapeRoot> as = worldShS.allShapes();
        for(int i=0; i < as.size(); i++) {
            if(linetest(as[i])) {
                as[i]->setObstacle(false);
                allShapes.push_back(as[i]);
            } else if(ellipsetest(as[i])) {
                as[i]->setObstacle(false);
                allShapes.push_back(as[i]);
            }
        }
        postStateCompletion();
    }

	$nodeclass LocateObjects : MapBuilderNode(MapBuilderRequest::worldMap) : doStart 
    {
        cout << "locating" << endl;
		mapreq.pursueShapes = false;
        mapreq.addObjectColor(ellipseDataType, "red");
        mapreq.addObjectColor(ellipseDataType, "blue");
        mapreq.addObjectColor(ellipseDataType, "green");
        
        mapreq.addObjectColor(lineDataType, "red");
        mapreq.addObjectColor(lineDataType, "blue");
        mapreq.addObjectColor(lineDataType, "green");
    }
        
    $nodeclass FilterEllipses : VisualRoutinesStateNode : doStart {
        $reference Executor::allShapes;
        IsType ellipsetest(ellipseDataType);
        for(unsigned int i=0; i < allShapes.size();) {
            if(!ellipsetest(allShapes[i]))
                allShapes.erase(allShapes.begin() + i);
            else
                i++;
        }
        postStateCompletion();
    }
    
    $nodeclass FilterLines : VisualRoutinesStateNode : doStart {
        $reference Executor::allShapes;
        IsType linetest(lineDataType);
        for(unsigned int i=0; i < allShapes.size();) {
            if(!linetest(allShapes[i])) {
                allShapes.erase(allShapes.begin() + i);
            } else {
                i++;
            }
        }
        postStateCompletion();
    }
        
	
    /*********************************************
     * Filter Shapes : Absolute
     *********************************************/
    
    $nodeclass FilterRange : VisualRoutinesStateNode : doStart {
        cout << "filtering range" << endl;
        Point robot = theAgent->getCentroid();
        vector<ShapeRoot> as = worldShS.allShapes();
        for(unsigned int i=0; i < as.size(); i++) {
            if(as[i]->getCentroid().distanceFrom(robot) > MAX_RANGE) {
                worldShS.deleteShape(as[i]);
                cout << "deleted something" << endl;
            } else {
                cout << "not deleting " << as[i]->getCentroid().distanceFrom(robot) << endl;
            }
        }
        postStateCompletion();
    }
    
    $nodeclass FilterClean : VisualRoutinesStateNode : doStart {
        IsType linetest(lineDataType);
        IsType ellipsetest(ellipseDataType);
        vector<ShapeRoot> as = worldShS.allShapes();
        for(unsigned int i=0; i < as.size(); i++) {
            if(ellipsetest(as[i])) {
                Point e_center = as[i]->getCentroid();
                for(unsigned int j=0; j < as.size(); j++) {
                    if(linetest(as[j]) && as[j]->getCentroid().distanceFrom(e_center) < 50) {
                        worldShS.deleteShape(as[j]);
                    }
                }
            }
        }
        postStateCompletion();
    }

    $nodeclass FilterRed : VisualRoutinesStateNode : doStart {
        $reference Executor::allShapes;
        IsColor redtest("red");
        for(unsigned int i=0; i < allShapes.size();) {
            if(!redtest(allShapes[i]))
                allShapes.erase(allShapes.begin() + i);
            else
                i++;
        }
        postStateCompletion();
    }
    
    $nodeclass FilterBlue : VisualRoutinesStateNode : doStart {
        $reference Executor::allShapes;
        IsColor bluetest("blue");
        for(unsigned int i=0; i < allShapes.size(); ) {
            if(!bluetest(allShapes[i]))
                allShapes.erase(allShapes.begin() + i);
            else
                i++;
        }
        postStateCompletion();
    }
    
    $nodeclass FilterGreen : VisualRoutinesStateNode : doStart {
        $reference Executor::allShapes;
        IsColor greentest("green");
        for(unsigned int i=0; i < allShapes.size(); ) {
            if(!greentest(allShapes[i]))
                allShapes.erase(allShapes.begin() + i);
            else
                i++;
        }
        postStateCompletion();
    }
    
    $nodeclass FilterClosest : VisualRoutinesStateNode : doStart {
        $reference Executor::allShapes;
        Point robot = theAgent->getCentroid();
        for(unsigned int i=1; i < allShapes.size(); ) {
            if(allShapes[i]->getCentroid().distanceFrom(robot) < allShapes[0]->getCentroid().distanceFrom(robot)) {
                cout << "found a smaller shape, deleting " << endl;
                allShapes.erase(allShapes.begin() + 0);
            } else {
                cout << "not smaller, deleting " << i << endl;
                allShapes.erase(allShapes.begin() + 1);
            }
        }
        postStateCompletion();
    }

    $nodeclass FilterLeftmost : VisualRoutinesStateNode : doStart {
        $reference Executor::allShapes;
        float ymax = VRmixin::mapBuilder->importWorldToLocal(allShapes[0])->getCentroid().coordY();
        for(unsigned int i=1; i < allShapes.size(); ) {
            ShapeRoot locShape = VRmixin::mapBuilder->importWorldToLocal(allShapes[i]);
            float thisy = locShape->getCentroid().coordY();
            if(thisy > ymax) {
                ymax = thisy;
                allShapes.erase(allShapes.begin() + 0);
            } else {
                allShapes.erase(allShapes.begin() + 1);
            }
        }
        postStateCompletion();
    }
    
    $nodeclass FilterRightmost : VisualRoutinesStateNode : doStart {
        $reference Executor::allShapes;
        float ymax = VRmixin::mapBuilder->importWorldToLocal(allShapes[0])->getCentroid().coordY();
        for(unsigned int i=1; i < allShapes.size(); ) {
            ShapeRoot locShape = VRmixin::mapBuilder->importWorldToLocal(allShapes[i]);
            float thisy = locShape->getCentroid().coordY();
            if(thisy < ymax) {
                ymax = thisy;
                allShapes.erase(allShapes.begin() + 0);
            } else {
                allShapes.erase(allShapes.begin() + 1);
            }
        }
        postStateCompletion();
    }

    $nodeclass FilterLargest : VisualRoutinesStateNode : doStart {
        $reference Executor::allShapes;

        Shape<BlobData> s = ShapeRootTypeConst(allShapes[0], BlobData);
        float areamax = s->getArea();
        for(unsigned int i=1; i < allShapes.size(); ) {
            s = ShapeRootTypeConst(allShapes[i], BlobData);
            float thisarea = s->getArea();
            if(thisarea > areamax) {
                areamax = thisarea;
                allShapes.erase(allShapes.begin() + 0);
            } else {
                allShapes.erase(allShapes.begin() + 1);
            }
        }
        postStateCompletion();
    }
    
    $nodeclass FilterSmallest : VisualRoutinesStateNode : doStart {
        $reference Executor::allShapes;

        Shape<BlobData> s = ShapeRootTypeConst(allShapes[0], BlobData);
        float areamax = s->getArea();
        for(unsigned int i=1; i < allShapes.size(); ) {
            s = ShapeRootTypeConst(allShapes[i], BlobData);
            float thisarea = s->getArea();
            if(thisarea < areamax) {
                areamax = thisarea;
                allShapes.erase(allShapes.begin() + 0);
            } else {
                allShapes.erase(allShapes.begin() + 1);
            }
        }
        postStateCompletion();
    }
    
    /*********************************************
     * Filter Shapes : Relative
     *********************************************/
    $nodeclass FilterLeftOfRelative : VisualRoutinesStateNode : doStart {
        $reference Executor::allShapes;
        $reference Executor::relativeShape;
        
        float yrel = VRmixin::mapBuilder->importWorldToLocal(relativeShape)->getCentroid().coordY();
        for(unsigned int i=0; i < allShapes.size(); ) {
            ShapeRoot locShape = VRmixin::mapBuilder->importWorldToLocal(allShapes[i]);
            float thisy = locShape->getCentroid().coordY();
            if(thisy > yrel) {
                i++;
            } else {
                allShapes.erase(allShapes.begin() + i);
            }
        }
        postStateCompletion();
    }
    
    $nodeclass FilterRightOfRelative : VisualRoutinesStateNode : doStart {
        $reference Executor::allShapes;
        $reference Executor::relativeShape;
        
        float yrel = VRmixin::mapBuilder->importWorldToLocal(relativeShape)->getCentroid().coordY();
        for(unsigned int i=0; i < allShapes.size(); ) {
            ShapeRoot locShape = VRmixin::mapBuilder->importWorldToLocal(allShapes[i]);
            float thisy = locShape->getCentroid().coordY();
            if(thisy < yrel) {
                i++;
            } else {
                allShapes.erase(allShapes.begin() + i);
            }
        }
        postStateCompletion();
    }
    
    
    /*********************************************
     * State Machine
     *********************************************/

    $nodeclass Init : StateNode : doStart {
        $reference Executor::validCmds;
        validCmds.push_back("BufferEmpty");
        validCmds.push_back("filter ball");
        validCmds.push_back("filter line");
        validCmds.push_back("filter red");
        validCmds.push_back("filter blue");
        validCmds.push_back("filter green");
        validCmds.push_back("filter smallest");
        validCmds.push_back("filter biggest");
        validCmds.push_back("filter closest");
        validCmds.push_back("filter leftmost");
        validCmds.push_back("filter rightmost");
        validCmds.push_back("filter left");
        validCmds.push_back("filter right");
        validCmds.push_back("remember");
        validCmds.push_back("go");
        validCmds.push_back("turn");
        validCmds.push_back("look");
        validCmds.push_back("grab");
        postStateCompletion();
    }
    
    $nodeclass Wait : StateNode : doStart {
        printf("\n\n\nWaiting...\n");
    }

    $nodeclass ProcessString : StateNode : doStart {
        $reference Executor::cmds;
        
        cmds.clear();
       
        const TextMsgEvent *txtev = dynamic_cast<const TextMsgEvent*>(event);
        if ( txtev == NULL ) {
            printf("Did not receive string\n");
            postStateCompletion();
        }
        string nlp = txtev->getText();
        printf("Process [%s]\n", nlp.c_str());


        // run the python based parser and retrieve results
        string exec = "python ../stanford-nlp/parse.py \"" + nlp + "\"";
        char buf[BUF_SIZE];

        FILE * fp = popen(exec.c_str(), "r");
        if(fp == NULL) {
            printf("Parsing Failed\n");
            postStateCompletion();
        }
        
        while(fgets(buf, BUF_SIZE-1, fp) != NULL) {
            string cmd(buf);
            cmd.erase(remove(cmd.begin(), cmd.end(), '\n'), cmd.end());
            printf("Read: %s\n", cmd.c_str());
            cmds.push_back(cmd);
        }
        pclose(fp);
        postStateCompletion();
    }

    $nodeclass ExecuteCommand : StateNode : doStart {
        $reference Executor::validCmds;
        $reference Executor::cmds;
        $reference Executor::allShapes;
        
        if(cmds.size() <= 0) {
            postStateSignal<string>("BufferEmpty");
            return;
        }

        cout << "Active set of " << allShapes.size() << " objects" << endl;

        string cmd = cmds[0];
        cmds.erase(cmds.begin() + 0);

        vector<string>::iterator it = find(validCmds.begin(), validCmds.end(), cmd);
        if(it == validCmds.end()) {
            postStateFailure();
        }

        printf("*************************************\n");
        printf("Executing cmd %s\n", cmd.c_str());
        printf("*************************************\n");
        postStateSignal<string>(cmd);

    }

    virtual void setup() {

        $statemachine{
            startdemo: StateNode =T(100)=> Init =C=> DynamicMotionSequenceNode("park.mot") =C=> PostureNode("head0.pos") =C=> LocateObjects =C=> PostureNode("head1.pos") =C=> LocateObjects =C=> PostureNode("head2.pos") =C=> LocateObjects =C=> PostureNode("head3.pos") =C=> LocateObjects =C=> PostureNode("head4.pos") =C=> LocateObjects =C=> PostureNode("head5.pos") =C=> LocateObjects =C=> PostureNode("head6.pos") =C=> LocateObjects =C=> PostureNode("head3.pos") =C=> FilterRange =C=> FilterClean =C=> StoreObjects =C=> wait
           // startdemo: StateNode =T(100)=> Init =C=> DynamicMotionSequenceNode("park.mot") =C=> PostureNode("head2.pos") =C=> LocateObjects =C=> PostureNode("head3.pos") =C=> LocateObjects =C=> PostureNode("head4.pos") =C=> LocateObjects =C=> PostureNode("head3.pos") =C=> FilterRange =C=> FilterClean =C=> StoreObjects =C=> wait
            
            execute : ExecuteCommand
            execute =S<string>("BufferEmpty")=> StoreObjects =C=> wait
            execute =S<string>("filter ball")=> FilterEllipses =C=> execute
            execute =S<string>("filter line")=> FilterLines =C=> execute
            execute =S<string>("filter red")=> FilterRed =C=> execute
            execute =S<string>("filter blue")=> FilterBlue =C=> execute
            execute =S<string>("filter green")=> FilterGreen =C=> execute
            execute =S<string>("filter biggest")=> FilterLargest =C=> execute
            execute =S<string>("filter smallest")=> FilterSmallest =C=> execute
            execute =S<string>("filter closest")=> FilterClosest =C=> execute
            execute =S<string>("filter leftmost")=> FilterLeftmost =C=> execute
            execute =S<string>("filter rightmost")=> FilterRightmost =C=> execute
            execute =S<string>("filter left")=> FilterLeftOfRelative =C=> execute
            execute =S<string>("filter right")=> FilterRightOfRelative =C=> execute
            execute =S<string>("remember")=> Remember =C=> StoreObjects =C=> execute
            execute =S<string>("go")=> go
            execute =S<string>("look")=> look
            execute =S<string>("turn")=> turn
            execute =S<string>("grab")=> gograb

            execute =F=> SpeechNode("Command not recognized") =C=> StoreObjects =C=> wait
            
            gograb : GoTo
            gograb =C=> grab1
            gograb =F=> SpeechNode("Error, Target Object not found") =C=> StoreObjects =C=> wait
            grab1 : GrabNode(1)
            grab0 : GrabNode(0)
            grab1 =C=> DynamicMotionSequenceNode("opengripper.mot") =C=> grab0
            grab1 =F=> SpeechNode("Error, Target Object not found") =C=> StoreObjects =C=> wait
            grab0 =C=> DynamicMotionSequenceNode("closegripper.mot") =C=> DynamicMotionSequenceNode("park.mot") =C=> execute

            look : LookAt
            look =C=> execute
            look =F=> SpeechNode("Error, Target Object not found") =C=> StoreObjects =C=> wait
            
            turn : TurnTo
            turn =C=> execute
            turn =F=> SpeechNode("Error, Target Object not found") =C=> StoreObjects =C=> wait

            go : GoTo
            go =C=> execute
            go =F=> SpeechNode("Error, Target Object not found") =C=> StoreObjects =C=> wait
            
            wait : Wait =TM=> ProcessString =C=> execute
        }
    }

}

REGISTER_BEHAVIOR(Executor);