#include "behaviortree_cpp/action_node.h"

namespace mg {

    class CanChooser : public BT::SyncActionNode {
        struct cans {
            double x;
            double y;
            int    orientation;
        };

      public:
        CanChooser(const std::string& name, const BT::NodeConfig config) : BT::SyncActionNode(name, config) { }

        static BT::PortsList providedPorts() {
            // This action has a single input port called "message"
            return { BT::InputPort<std::string>("canlist"),
                     BT::OutputPort<double>("x_loc"),
                     BT::OutputPort<double>("y_loc"),
                     BT::OutputPort<int>("orient") };
        }

        // You must override the virtual function tick()
        BT::NodeStatus tick() override {
            constexpr std::array<cans, 10> c{ { { 0.825, 1.425, 3 },
                                                { 0.35, 0.4, 2 },
                                                { 0.35, 1.325, 2 },
                                                { 0.775, 0.55, 1 },
                                                { 2.225, 0.55, 1 },
                                                { 2.65, 0.4, 0 },
                                                { 2.65, 0.4, 0 },
                                                { 2.175, 1.425, 3 },
                                                { 1.1, 0.65, 3 },
                                                { 1.9, 0.65, 3 } } };

            int idx = 0;
            // Todo iterate through all cans and use a passthrough function that returns current can state
            std::istringstream ss(getInput<std::string>("canlist").value());
            ss >> idx;

            setOutput<double>("x_loc", c[idx].x);
            setOutput<double>("y_loc", c[idx].y);
            setOutput<int>("orient", c[idx].orientation);

            return BT::NodeStatus::SUCCESS;
        }
    };
}