#include "Behaviors/StateMachine.h"

$nodeclass DragTest : VisualRoutinesStateNode {

    $provide float destDx;
    $provide AngTwoPi orientation;


    $nodeclass GoToPoint : PilotNode(PilotTypes::walk) : doStart {
    
        $reference DragTest::destDx;
        $reference DragTest::orientation;
        pilotreq.dx =  destDx;
        pilotreq.da = orientation;
    }


    $nodeclass CalculatePosition : MapBuilderNode : doStart {
   
        $reference DragTest::orientation;
        $reference DragTest::destDx;

        //test target
        Point target = Point(1000, 0, 0);

        Point currRobotPosition = theAgent->getCentroid();
        cout << "currRobotPosition: " << currRobotPosition << endl;
        fmat::Column<3ul, float> gripperPositionColumn = kine->getPosition(GripperFrameOffset);

        Point currGripperPosition = Point(gripperPositionColumn[0], gripperPositionColumn[1], gripperPositionColumn[2]);       
        
        float destXDist = target.coordX() - currRobotPosition.coordX();
        float destYDist = target.coordY() - currRobotPosition.coordY();

        float r = sqrt(pow(currGripperPosition.coordX(),2) + pow(currGripperPosition.coordY(),2));
        
        destDx = sqrt(pow(destXDist, 2) + pow(destYDist, 2)) - r;
  
        //this is the source of the bug when b = 0
        //it should be set to something else which takes into account gripperframeoffset 
        float a = destXDist;
        float b = destYDist;

        /*
         * there are 3 solutions to the partial derivative of
         * theta = (x_g - x_t)^2 + (y_g - y_t)^2
         * we need to test which one we want
        */ 
        float theta;
        if (a != 0 && b == 0) {
            theta = 2 * M_PI;
        }
        else {

            if (b != 0 && a*sqrt(pow(a,2) + pow(b,2)) - pow(a,2) - pow(b,2) != 0)
                 theta = 2.0 * (atan2((sqrt(pow(a,2) + pow(b,2)) - a), b));
            else  theta = 2.0 * (atan2((-sqrt(pow(a,2) + pow(b,2)) - a), b));
        }
                 
        orientation = AngTwoPi(theta); 
        postStateCompletion();
    
    }

    $setupmachine {

        CalculatePosition =C=> GoToPoint

    }
}

REGISTER_BEHAVIOR(DragTest);