#include "Behaviors/StateMachine.h"

#define PAN_TO_ROT M_PI/2
#define DISTANCE_OFFSET 100

$nodeclass Lab7p5 : VisualRoutinesStateNode {
    $provide Point e;


    // Looks at scene and extracts ellipses
    $nodeclass AddToMap : MapBuilderNode(MapBuilderRequest::worldMap) : doStart {
        cout << "adding to my map" << endl;
        mapreq.addObjectColor(ellipseDataType, "blue");
        cout << "done adding to my map" << endl;
    }

	$nodeclass ExtractEllipse : VisualRoutinesStateNode : doStart {
        $reference Lab7p5::e;

        NEW_SHAPEVEC(ellipses, EllipseData, select_type<EllipseData>(worldShS));

        if(ellipses.size() < 1) {
            cout << "didn't find ellipse" << endl;
            return;
        }

        e = ellipses[0]->getCentroid();
        cout << "Ellipse found at " << e << endl;

    }
	
    $nodeclass TrackPan : PostureNode : doStart {
        $reference Lab7p5::e;
        
        cout << "TrackPan knows about ellipse " << e << endl;

        float pan = state->outputs[HeadOffset + PanOffset];

        cout << "RotPan:" << pan << endl;
		
        fmat::Quaternion oriTgt = fmat::Quaternion::aboutZ(pan * PAN_TO_ROT);

		fmat::Column<3> target = fmat::pack(e.coordX(),e.coordY(),16);
		fmat::Column<3> posOffset = fmat::pack(-DISTANCE_OFFSET,0,0);
		fmat::Quaternion oriOffset = fmat::Quaternion::identity();
		cout << "ArmElbow max speed=" << getMC()->getMaxSpeed(ArmElbowOffset) << endl;
		bool success = getMC()->solveLink(target, oriTgt, GripperFrameOffset, posOffset, oriOffset);
		cout << "success=" << success << endl;

        
	}


  $setupmachine{
     PostureNode("handeyelookdown.pos") =C=> AddToMap =C=> ExtractEllipse =T(1000)=> tp
    
     tp : TrackPan =C=> tp

  }

}

REGISTER_BEHAVIOR(Lab7p5);