//  BlockyFroggy
//  Copyright © 2017 John Ryland.
//  All rights reserved.
#pragma once
#ifndef BEHAVIOR_TREE_H
#define BEHAVIOR_TREE_H


/*!
@page Topics
@subpage BehaviorTrees
@page BehaviorTrees Behavior Trees

A tree is composed of nodes.

The nodes can be leaf nodes that contain behaviors, or branch nodes.

Leaf nodes contain the behavior logic, and are executed. This can
result in success or failure, and can be run over multiple ticks in
which case if it doesn't complete in a given tick will return a status
that it is still running.

The branch nodes can be one of:
  - Selector
  - Sequence or
  - Parallel

Branch nodes contain a collection of children nodes which can be
more branch nodes and/or leaf nodes.

Selectors will find the first child that can be executed successfully
and will execute it.

Sequences will execute each child in sequence until the first child
fails.

Parallel will execute the nodes as if they were executed in parallel
until the first child node completes and returns the status of the
first node. (This may be subject to change).

Associated with each node is a decision. The decision node is used
to decide if the associated behavior node should be executed or not.
It may be subject to change as to if this is counted as failure or
not in the above branch nodes. Possibly it does.

Together these collectively form a tree which is encompassed in the
behavior tree class.

Insperation:
http://mfg.rethinkrobotics.com/intera/Parallel_Node
*/


#include "Common.h"
#include <queue>
#include <mutex>
#include <memory>
#include <vector>


class BehaviorNode
{
public:
  enum Status
  {
    Running,
    Success,
    Failure
  };
  virtual const char* Name() { return "base-behavior"; }
  virtual Status Execute() {}
};
// Need a factory thing for creating nodes from a name


class DecisionNode
{
public:
  virtual bool ShouldExecute() {}
};


// Idea is to encapsulate a node in a decorator node to
// do things like reverse the logic, ie a NOT gate or things like that
class DecoratorNode
{
};


class BehaviorAndDecision
{
  std::shared_ptr<BehaviorNode> m_behavior;
  std::shared_ptr<DecisionNode> m_decision;
};


class BehaviorBranch : public BehaviorNode, DecisionNode
{
private:
  std::vector<BehaviorAndDecision> m_children;
};


class SelectorNode : public BehaviorBranch
{
  const char* Name() override { return "selector"; }
  Status Execute() override
  {
    for (auto node : m_children)
    {
      Status status = node.m_decision->Execute();
      if (status == Running)
      {
        return Running;
      }
      if (status == Success)
      {
        return Success;
      }
    }
    return Failure;
  }
};


class ParallelNode : public BehaviorBranch
{
  const char* Name() override { return "parallel"; }
  Status Execute() override
  {
    // TODO: this is not the correct implementation for parallel
    // http://mfg.rethinkrobotics.com/intera/Parallel_Node
    for (auto node : m_children)
    {
      Status status = node.m_decision->Execute();
      if (status == Running)
      {
        return Running;
      }
      if (status == Failure)
      {
        return Failure;
      }
    }
    return Success;
  }
};


class SequenceNode : public BehaviorBranch
{
  const char* Name() override { return "sequence"; }
  Status Execute() override
  {
    for (auto node : m_children)
    {
      Status status = node.m_decision->Execute();
      if (status == Running)
      {
        return Running;
      }
      if (status == Failure)
      {
        return Failure;
      }
    }
    return Success;
  }
};


class BehaviorTree
{
public:

private:
  std::shared_ptr<BehaviorNode> m_head;
};


#endif // BEHAVIOR_TREE_H