#include <cmath>

#include "Shared/RobotInfo.h"
#include "Behaviors/StateMachine.h"
#include "Motion/MotionPtr.h"

$nodeclass GripperDemo {
    $provide Point egg_center;

    $nodeclass EggFinder : MapBuilderNode(MapBuilderRequest::worldMap) : constructor {
        mapreq.addAllObjectColors(cylinderDataType);
    }

    $nodeclass ArmMover : PostureNode : doStart {
        $reference GripperDemo::egg_center;
        Point center_unit = egg_center.unitVector();
        fmat::Column<3> egg_offset = (Point(egg_center.coordX(), egg_center.coordY(), egg_center.coordZ()) - center_unit*100).getCoords();
        fmat::Column<3> gripper_offset = fmat::pack(0, 0, 0);
#ifdef TGT_IS_CALLIOPE5
        unsigned int link = LeftFingerFrameOffset;
#else
        unsigned int link = GripperFrameOffset;
#endif
        getMC()->solveLinkPosition(egg_offset, link, gripper_offset);
    }

    $nodeclass EggPusher : PostureNode : doStart {
        $reference GripperDemo::egg_center;
        Point center_unit = egg_center.unitVector();
        fmat::Column<3> egg_offset = (Point(egg_center.coordX(), egg_center.coordY(), egg_center.coordZ()) + center_unit*200).getCoords();
        fmat::Column<3> gripper_offset = fmat::pack(0, 0, 0);
#ifdef TGT_IS_CALLIOPE5
        unsigned int link = GripperFrameOffset;
#else
        unsigned int link = GripperFrameOffset;
#endif
        getMC()->solveLinkPosition(egg_offset, link, gripper_offset);
    }

    $nodeclass EggPicker : doStart {
        $reference GripperDemo::egg_center;
        Shape<CylinderData> gripper_egg;
        NEW_SHAPEVEC(eggs, CylinderData, select_type<CylinderData>(localShS));
        float arm_max = VRmixin::theAgent->getBoundingBox().getDimensions()[0];
        bool egg_found = false;
        SHAPEVEC_ITERATE(eggs, CylinderData, egg) {
            if(egg->getCentroid().coordX() > 0 && egg->getCentroid().coordX() < arm_max &&
                fabs(egg->getCentroid().coordY()) < arm_max) {
                gripper_egg = egg;
                egg_found = true;
                break;
            }
        } END_ITERATE;
        if(!egg_found) {
            postStateFailure();
            return;
        }
        egg_center = gripper_egg->getCentroid();
        postStateCompletion();
    }

    virtual void setup() {
        MotionManager::MC_ID posture_mc = addMotion(MotionPtr<PostureMC>());
        $statemachine {
            startnode: EggFinder =C=>
                  picker =C=>
                  ArmMover[setMC(posture_mc)] =C=>
                  EggPusher[setMC(posture_mc)] =C=>
                  StateNode =B(PlayButOffset)=>
                  startnode
            picker =F=> SpeechNode("failure") =B(PlayButOffset)=> startnode
            picker: EggPicker
        }
    }
}

REGISTER_BEHAVIOR(GripperDemo);