/* 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 WMStructs_h
#define WMStructs_h

#include <deque>

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

struct BallObs {
  vector2d loc;
  vector2d vel;
  ulong timestamp;

  // Pad structure so it's 8 byte aligned so it may be
  // nested in other structs and look the same under
  // Linux and Aperios.
  unsigned char padding[4];
};


/* We share our own location in robot observations. Our
   distances to robots are pretty bad. The bearing readings
   should be fairly good. So we can be fairly confident that
   there is indeed a robot along the line from my_loc to the
   edge of the field along bearing.
*/
struct RobotObs {
  vector2d my_loc;
  double bearing;
  double distance;
  ulong timestamp;
  uchar robot_color;

  bool operator<(const RobotObs &other) const { return timestamp < other.timestamp; }

  unsigned char padding[3];
};

/* We want to consider different possible time windows */
#if 0
template <class T>
class ObservationStore {

  std::deque<T> observations;

  bool valid_window;
  ulong window_start;
  ulong window_end;

 public:
  
  // O(n) insertion into list
  void insert(T obs) {
    typename std::deque<T>::iterator first_ind = observations.begin();
    typename std::deque<T>::iterator insert_ind = observations.end();

    observations.push_back(obs);
    while(insert_ind != first_ind &&
    	  (*insert_ind) < (*(insert_ind - 1))) {
      T temp = (*insert_ind);
      (*insert_ind) = (*(insert_ind - 1));
      (*(insert_ind - 1)) = temp;
      insert_ind--;
    }
  }

  void removeOlderThan(ulong timestamp) {
    typename std::deque<T>::iterator index = observations.begin();
    
    while(index!=observations.end() &&
	  (*index).timestamp < timestamp) {
      observations.pop_front();
      index = observations.begin();
    }
  }
}; 
#endif

#endif