/* LICENSE:
  =========================================================================
    CMPack'04 Source Code Release for OPEN-R SDK 1.1.5-r2 for ERS7
    Copyright (C) 2004 Multirobot Lab [Project Head: Manuela Veloso]
    School of Computer Science, Carnegie Mellon University
    All rights reserved.
  ========================================================================= */

#include "BehTree.h"
#include "../headers/CircBufPacket.h"

BehTree::BehTree() {
  root = NULL;
}

BehTree::~BehTree() {

  if(root!=NULL) {
    for(int i=0; i< root->num_children;i++){
      if(root->children[i]!=NULL)
 	delete root->children[i];
      root->children[i] = NULL;
    }
    delete root;
  }
}

//return a new decision node that splits on an integer value
BehTree::TreeNode* BehTree::addDecisionNode(char* name, int num_vals, int *value_ptr)
{
  BehTree::TreeNode *node = new TreeNode();
  
  node->attr_name = name;
  node->value_type = T_INT;
  node->bool_attr_value = NULL;
  node->int_attr_value = value_ptr;

  node->beh_seq = NULL;
  node->num_successes = node->num_failures = 0;

  node->num_children = num_vals;
  node->children = new TreeNode*[num_vals];
  for(int i = 0; i < num_vals;i++)
    node->children[i] = NULL;

  return node;

}

//return a new decision node that splits on a boolean value
BehTree::TreeNode* BehTree::addDecisionNode(char* name, int num_vals, bool *value_ptr)
{
  BehTree::TreeNode *node = new TreeNode();
  
  node->attr_name = name;
  node->value_type = T_BOOL;
  node->bool_attr_value = value_ptr;
  node->int_attr_value = NULL;

  node->beh_seq = NULL;
  node->num_successes = node->num_failures = 0;

  node->num_children = num_vals;
  node->children = new TreeNode*[num_vals];
  for(int i = 0; i < num_vals;i++)
    node->children[i] = NULL;

  return node;

}

//return a new leaf node that specifies which BehaviorSequence to execute
BehTree::TreeNode* BehTree::addLeafNode(char* beh_seq)
{
  BehTree::TreeNode *node = new TreeNode();
  
  node->attr_name = beh_seq;
  node->beh_seq = beh_seq;
  node->num_successes = node->num_failures = 0;
  
  node->value_type = -1;
  node->bool_attr_value = NULL;
  node->int_attr_value = NULL;

  node->num_children = 0;
  node->children = NULL;
  
  return node;

}


void BehTree::print() {
  pprintf(TextOutputStream,"Behavior Tree: \n");
  recursivePrint(0,root);
  pprintf(TextOutputStream,"\n");
}

static char *PrintIndent(int indent_level,char *buf) {
  for(int i=0; i<indent_level; i++)
    buf[i] = ' ';
  return buf+indent_level;
}

void BehTree::recursivePrint(int indent_level,TreeNode *node) {
  static char buf[2048];
  char *bufp=buf;

  if(node==NULL){
    bufp = PrintIndent(indent_level,buf);
    sprintf(bufp,"TreeNode = NULL");
    pprintf(TextOutputStream,"%s\n",buf);
    return;
  }
    
  if(node->num_children > 0){ //decision node

    for(int i=0;i<node->num_children;i++){
      bufp = PrintIndent(indent_level,buf);
      sprintf(bufp,"%s = %d",node->attr_name,i);
      pprintf(TextOutputStream,"%s\n",buf);
      
      recursivePrint(indent_level+2,node->children[i]);
    }
  }else{
    bufp = PrintIndent(indent_level,buf);
    sprintf(bufp,"run: \"%s\" (%dS,%dF)",node->beh_seq,node->num_successes, node->num_failures);
    pprintf(TextOutputStream,"%s\n",buf);
  }
  
}