#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 10000

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

    /*********************************************
     * Actions
     *********************************************/
    
    $nodeclass GoTo : PilotNode(PilotTypes::goToShape) : doStart {
        $reference Executor::allShapes;
        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) - 400.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);

        cout << "going" << endl;
        pilotreq.targetShape = dest;
        pilotreq.collisionAction = collisionIgnore;
        cout << "gone" << endl;
    }



    /*********************************************
     * 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])) {
                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 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();
    }
    
    /*********************************************
     * 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 WaitForCommand : StateNode : doStart {
        $reference Executor::allShapes;
        cout << "Active set of " << allShapes.size() << " objects" << endl;
        cout << "waiting..." << endl;
    }

    virtual void setup() {

        $statemachine{
            
            startdemo: StateNode =T(100)=> PostureNode("parked.pos") =C=> wait
            wait: WaitForCommand
            wait =TM("LocateObjects")=> 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
            wait =TM("ResetFilters")=> StoreObjects =C=> wait
            wait =TM("FilterEllipses")=> FilterEllipses =C=> wait
            wait =TM("FilterLines")=> FilterLines =C=> wait
            wait =TM("FilterRed")=> FilterRed =C=> wait
            wait =TM("FilterBlue")=> FilterBlue =C=> wait
            wait =TM("FilterLargest")=> FilterLargest =C=> wait
            wait =TM("FilterClosest")=> FilterClosest =C=> wait
            wait =TM("FilterLeftmost")=> FilterLeftmost =C=> wait
            wait =TM("FilterRightmost")=> FilterRightmost =C=> wait
            wait =TM("FilterLeftOf")=> FilterLeftOfRelative =C=> wait
            wait =TM("FilterRightOf")=> FilterRightOfRelative =C=> wait
            wait =TM("Remember")=> Remember =C=> wait

            wait =TM("GoTo")=> GoTo =C=> wait
        }
    }

}

REGISTER_BEHAVIOR(Executor);