#include "Behaviors/StateMachine.h"

$nodeclass DragDemo : VisualRoutinesStateNode {

    $provide Shape<CylinderData> currTarget;
    $provide size_t numCylindersMoved;

    $setupmachine {
         
                SetUp =N=> GenerateMap =N=> findCylinders : FindCylinders
                findCylinders =N=> GrabCylinder =C=> PlaceCylinder =C=> findCylinders 
    }

    $nodeclass GenerateMap : StateNode : doStart {

        //walls containing the three cylinders
        vector<Point> wallPts;
        //these pts are offset
        wallPts.push_back(Point(850, -350, 0, allocentric));
        wallPts.push_back(Point(1650, -350,0, allocentric));
        wallPts.push_back(Point(1650, 250, 0, allocentric));
        wallPts.push_back(Point(850, 250, 0, allocentric));

        NEW_SHAPE(wall, PolygonData, new PolygonData(worldShS, wallPts, false));

        //cylinder positions

        fmat::QuaternionT<float> identity = fmat::Quaternion();

        float radius = 50; 

        NEW_SHAPE(redCylinder, CylinderData, new CylinderData(worldShS, Point(1485,-50, 200.f/2, allocentric), 200, radius));
	redCylinder->setColor("red");

        NEW_SHAPE(greenCylinder, CylinderData, new CylinderData(worldShS, Point(1285,-50, 132.f/2, allocentric), 132, radius));
	greenCylinder->setColor("green");

        NEW_SHAPE(blueCylinder, CylinderData, new CylinderData(worldShS, Point(1085,-50, 90.f/2, allocentric), 90, radius));
	blueCylinder->setColor("blue");

        //target locations -- they appear as circles, but in fact are spheres with collision = false

        NEW_SHAPE(targetForRed, PointData, new PointData(worldShS, Point(-500, 300 , 0 , allocentric)));

        NEW_SHAPE(targetForGreen, PointData, new PointData(worldShS, Point(-500, 500, 0 , allocentric)));

        NEW_SHAPE(targetForBlue, PointData, new PointData(worldShS, Point(-500, 700, 0, allocentric)));

        //turn off obstacle attribute -- speeds up path planning
        targetForRed->setObstacle(false);
        targetForGreen->setObstacle(false);
        targetForBlue->setObstacle(false);
        
        cout << "worldmap completed" << endl;
    }   


    $nodeclass SetUp : StateNode : doStart {

        $reference DragDemo::numCylindersMoved;
        numCylindersMoved = 2;
        cout << "setup completed" << endl;
        cout << "posted completion for setup" <<endl;
    }

/*
    $nodeclass GoToStartLocation : PilotNode(PilotTypes::goToShape) : doStart {

        NEW_SHAPE(targetLoc, PointData, new PointData(worldShS, Point(500, 0, allocentric)));
        pilotreq.targetShape = targetLoc;
        pilotreq.obstacleInflation = 0.0f;
        //turn us so we can see the cylinders
        pilotreq.targetHeading = 0.0;
        cout << "go to start location completed" << endl;

    }
*/
    $nodeclass FindCylinders : VisualRoutinesStateNode : doStart {

        $reference DragDemo::currTarget;
        $reference DragDemo::numCylindersMoved;
        NEW_SHAPEVEC(cylinders, CylinderData, select_type<CylinderData>(worldShS));
        cout << "cylinders array size: " << cylinders.size() << endl;
        if (cylinders.size() > numCylindersMoved) {
            cout << "target shape set: "  <<  cylinders[numCylindersMoved] << endl;
            currTarget = cylinders[numCylindersMoved];
        }

    }

    $nodeclass GrabCylinder : GrasperNode(GrasperRequest::grasp) : doStart {

        $reference DragDemo::currTarget;

        cout << "currTarget = " << currTarget << endl;
        graspreq.object = currTarget; 
        cout << "grasper req target set" << endl;

    }

    $nodeclass PlaceCylinder : GrasperNode(GrasperRequest::moveTo) : doStart {

        $reference DragDemo::currTarget;
        $reference DragDemo::numCylindersMoved;

        NEW_SHAPE(targetLoc, PointData, new PointData(worldShS, Point(300 + (200 * numCylindersMoved), -500, 0, allocentric)));
        numCylindersMoved--;

	graspreq.object = currTarget;
        graspreq.targetLocation = targetLoc;

        graspreq.openGripperOnRest = true;

    }

}

REGISTER_BEHAVIOR(DragDemo);