#include "Extensions.hpp"
#include "Area.h"
#include <behaviortree_cpp_v3/basic_types.h>
#include <behaviortree_cpp_v3/blackboard.h>
#include <behaviortree_cpp_v3/bt_factory.h>
#include <boost/exception/exception.hpp>
#include <geometry_msgs/msg/detail/pose__struct.hpp>
#include <rclcpp/logging.hpp>
#include <rclcpp/rclcpp.hpp>
#include <random>
#include <rclcpp/utilities.hpp>
#include <std_msgs/msg/bool.hpp>
#include "nodes/WeightedRandomNode.h"
#include "nodes/GenerateXYPose.h"
#include "nodes/RobotMove.h"
#include "nodes/OffsetPose.h"
#include "nodes/RandomFailure.h"
#include "nodes/InAreaTest.h"
#include "nodes/InterruptableSequence.h"
#include "nodes/SetRobotVelocity.h"
#include "nodes/ActorMovement.h"
#include "nodes/ActorAnimation.h"
#include <chrono>
#include <thread>

using namespace BT;

int main(int argc, char **argv) {
    rclcpp::init(argc, argv);
    rclcpp::NodeOptions node_options;
    
    convertFromString<std::shared_ptr<geometry_msgs::msg::Pose>>("0,0,0");

    auto mainNode = rclcpp::Node::make_shared("tree_general_node", node_options);
    
    mainNode -> declare_parameter("trees",std::vector<std::string>({}));
    mainNode -> declare_parameter("areas",std::vector<std::string>({}));
    
    auto blackboard = Blackboard::create();
    auto blackboardMutex = std::mutex();

    rclcpp::executors::SingleThreadedExecutor executor;

    BehaviorTreeFactory factory;

    std::cout << "Registering nodes." << std::endl;

    factory.registerNodeType<GenerateXYPose>("GenerateXYPose");

    factory.registerNodeType<WeightedRandomNode>("WeightedRandom");
    factory.registerNodeType<OffsetPose>("OffsetPose");
    factory.registerNodeType<RandomFailure>("RandomFailure");
    factory.registerNodeType<InAreaTest>("InAreaTest");

    factory.registerNodeType<InterruptableSequence>("InterruptableSequence");

    auto moveNode = rclcpp::Node::make_shared("tree_moveit_node", node_options);
    auto connection = std::make_shared<MoveConnection>(moveNode, "arm");
    executor.add_node(moveNode);

    factory.registerBuilder<RobotMove>("RobotMove", [connection](const std::string &name, const NodeConfiguration &config) {
        return std::make_unique<RobotMove>(name, config, connection);
    });

    factory.registerBuilder<SetRobotVelocity>("SetRobotVelocity", [connection](const std::string &name, const NodeConfiguration &config) {
        return std::make_unique<SetRobotVelocity>(name, config, connection);
    });

    
    auto actorAnimationNode = rclcpp::Node::make_shared("actorAnimationNode",node_options);
    NodeBuilder builderActorAnimation = [&actorAnimationNode](const std::string &name, const NodeConfiguration &config) {
        return std::make_unique<ActorAnimation>(name, config, actorAnimationNode, "/ActorPlugin/animation");
    };
    factory.registerBuilder<ActorAnimation>("ActorAnimation",builderActorAnimation);
    executor.add_node(actorAnimationNode);

    auto actorMovementNode = rclcpp::Node::make_shared("actorMovementNode",node_options);
    NodeBuilder builderActorMovement = [&actorMovementNode, &blackboard, &blackboardMutex](const std::string &name, const NodeConfiguration &config) {
        return std::make_unique<ActorMovement>(name, config, actorMovementNode, "/ActorPlugin/movement",[&blackboard,&blackboardMutex](std::shared_ptr<const Movement::Feedback> feedback){
            blackboardMutex.lock();
            blackboard->set("actorPos", std::make_shared<geometry_msgs::msg::Pose>(feedback->current));
            blackboardMutex.unlock();
        });
    };
    factory.registerBuilder<ActorMovement>("ActorMovement",builderActorMovement);
    executor.add_node(actorMovementNode);

    bool called = false;
    auto IsCalled = [&called](__attribute__((unused)) TreeNode& parent_node) -> NodeStatus{
        return called ? NodeStatus::SUCCESS : NodeStatus::FAILURE;
    };

    auto SetCalledTo = [&called,&mainNode](TreeNode& parent_node) -> NodeStatus{
        if(!parent_node.getInput<bool>("state",called)){
            RCLCPP_DEBUG_STREAM(mainNode->get_logger(),"no state supplied.");
            return NodeStatus::FAILURE;
        }
        RCLCPP_DEBUG_STREAM(mainNode->get_logger(),"state changed.");
        return NodeStatus::SUCCESS;
    };

    factory.registerSimpleCondition("IsCalled",IsCalled);
    factory.registerSimpleAction("SetCalledTo",SetCalledTo,{InputPort("state","state to set called to")});
    
    RCLCPP_DEBUG_STREAM(mainNode->get_logger(), "Creating tree.");
    
    auto areaDefinitions = mainNode -> get_parameter("areas").as_string_array();
    if(areaDefinitions.size()%2==1){
        RCLCPP_ERROR_STREAM(mainNode->get_logger(), "Expected area syntax is \"name\",\"values\"");
        return -1;
    }
    for (unsigned long index = 0; index<areaDefinitions.size(); index+=2) {
        RCLCPP_INFO_STREAM(mainNode->get_logger(), "Got area \""<< areaDefinitions[index] <<"\"");
        blackboard->set(areaDefinitions[index], BT::convertFromString<std::shared_ptr<Area>>(areaDefinitions[index+1]));
    }
    
    auto treeFileNames = mainNode -> get_parameter("trees").as_string_array();
    auto trees = std::vector<std::shared_ptr<BT::Tree>>();
    for(auto treeFileName : treeFileNames){
        trees.emplace_back(std::make_shared<BT::Tree>(factory.createTreeFromFile(treeFileName,blackboard)));
    }

    auto init = std::make_shared<geometry_msgs::msg::Pose>();
    init->position.x = 6.0;
    init->position.y = 6.0;

    blackboard->set("actorPos", init);

    RCLCPP_INFO(mainNode->get_logger(), "Starting subscriber.");

    std::thread([&executor,&mainNode]() {
        while(rclcpp::ok()){
            executor.spin();
        }
        RCLCPP_WARN(mainNode->get_logger(), "Executor stopped.");
    }).detach();
    
    RCLCPP_INFO(mainNode->get_logger(), "Looping.");
    
    while (rclcpp::ok()) {
        blackboardMutex.lock();
        for(auto tree : trees){
            tree->tickRoot();
        }
        blackboardMutex.unlock();
    }

    RCLCPP_WARN(mainNode->get_logger(), "End.");
    
    rclcpp::shutdown();

    return 0;
}