/* 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 "DTree.h"
#include "../headers/CircBufPacket.h"

DTree::DTree() {
  head = NULL;
}

DTree::DTree(char *filename) {
  head = NULL;
  load(filename);
}

DTree::~DTree() {

  if(head!=NULL) {
    if(head->left!=NULL)
      delete head->left;
    
    if(head->right!=NULL)
      delete head->right;

    head->right = head->left = NULL;

    delete head;
  }
}

void DTree::load(char *filename) {

  HFS::FILE *infile = HFS::fopen(filename, "rb");

  if(infile==NULL) {
    head = NULL;
    pprintf(TextOutputStream, "Unable to open %s\n", filename);
    return;
  }

  head = recursiveLoad(infile);

  HFS::fclose(infile);
}

DTree::Node *DTree::recursiveLoad(HFS::FILE *infile) {

  Node *retval = new Node();
  
  HFS::fread(&retval->type, 1, sizeof(int32), infile);
  
  if(retval->type==Node::T_CHOICE) {
    HFS::fread(&retval->test_variable, 1, sizeof(int32), infile);
    HFS::fread(&retval->test_value, 1, sizeof(double), infile);
    
    retval->left = recursiveLoad(infile);
    retval->right = recursiveLoad(infile);
    
  } else if(retval->type==Node::T_LEAF) {
    HFS::fread(&retval->leaf_class, 1, sizeof(int32), infile);
  } else { 
    pprintf(TextOutputStream, 
	    "Invalid type %d when reading tree\n", retval->type);
  }

  return retval;
}

void DTree::print() {
  recursivePrint(0,head);
}

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

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

  if(node==NULL){
    bufp = PrintIndent(indent_level,buf);
    sprintf(bufp,"class=-1");
    pprintf(TextOutputStream,"%s\n",buf);
    return;
  }

  if(node->type == Node::T_CHOICE){
    bufp = PrintIndent(indent_level,buf);
    sprintf(bufp,"if(%d <= %g){",(int)node->test_variable,node->test_value);
    pprintf(TextOutputStream,"%s\n",buf);

    recursivePrint(indent_level+2,node->left);

    bufp = PrintIndent(indent_level,buf);
    sprintf(bufp,"}else{");
    pprintf(TextOutputStream,"%s\n",buf);

    recursivePrint(indent_level+2,node->right);

    bufp = PrintIndent(indent_level,buf);
    sprintf(bufp,"}");
    pprintf(TextOutputStream,"%s\n",buf);
  }else{
    bufp = PrintIndent(indent_level,buf);
    sprintf(bufp,"class=%d",(int)node->leaf_class);
    pprintf(TextOutputStream,"%s\n",buf);
  }
}

int32 DTree::getClass(double *features) {
  return getClass(features, -1); // -1 = no bounds check
}

/* Negative values of num_features are treated like
   no limit so bounds checking won't be done in those
   cases.
*/
int32 DTree::getClass(double *features, int32 num_features) {

  Node *current = head;

  while(current!=NULL &&
	current->type!=Node::T_LEAF) {

    // If testing bounds and we're out of bounds,
    // complain. Else make choice.
    if(num_features>=0 &&
       current->test_variable >= num_features) {
      pprintf(TextOutputStream, "Feature in tree out of bounds.\n");
      current = NULL;
    } else {
      if(features[current->test_variable] <= 
	 current->test_value)
	current = current->left;
      else
	current = current->right;
    }
  }
  
  if(current==NULL)
    return -1;
  else
    return current->leaf_class;
}