/* 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.
  ========================================================================= */

#ifndef StuckDetector_h
#define StuckDetector_h

#include "../headers/Geometry.h"
#include "DTree.h"

struct StuckInfo {
  enum StuckClass { WALL, PLAYING, HOOKED, STANDING };
  
  int last_state;
  double fraction_stuck;
};

class StuckDetector {

 public:
  

 private:

  // Number of StuckClass samples we keep around for reporting mean "stuck-ness"
  // We only track the result every test_interval sensor
  // frames (which happen 125 times/sec)
  static const int max_result_history = 50;

  // number of samples from gsensor history that we track.
  static const int max_history = 125;
  static const int num_features = 6;

  /* Constants that tell us how we use the decision tree */
  
  // How many sensor frames pass before we apply the decision tree to
  // get a classification?
  static const int test_interval = 5;
  
  DTree *tree;
  
  // Most recent test returned by the tree
  int last_class;
  int *result_history;
  int result_hist_index;
  int num_stuck_in_hist;

  // Count sensor frames between tests in the tree
  int test_timer;

  // data is the raw gsensor data
  vector3d *data;

  // The next point to be replaced in our data buffers.
  int data_index;
  
  // descriptive stats for the current window of max_history
  // points.
  vector3d history_mean;
  vector3d history_sum;
  double history_xx_sum;
  double history_yy_sum;
  double history_zz_sum;
  double history_xy_sum;
  double history_xz_sum;
  double history_yz_sum;

  // Features we pass to the decision tree. 
  // var.x, var.y, var.z, xy_cor, xz_cor, yz_cor
  double *features;

 public:

  StuckDetector();
  ~StuckDetector();
  void init();

  void update(vector3d accel);

  StuckInfo getStuckInfo();
  double getStuckFraction();
};

#endif