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

#include "../../headers/system_config.h"

#ifdef PLATFORM_LINUX
#include <iostream>
#endif

#include <string>

class Environment;
class LocalizationEngine;
class Motion::MotionLocalizationUpdate;
class MotionUpdateHeadSummary;
class RobotPositionInfo;
class SensorInfo;
class VisionInterface::ObjectInfo;
class Vision;

#include "Constants.h"

#include "../LocalizationInterface.h"
#include "LocalizationEngine.h"

class SRLocalizer {
public:
  SRLocalizer();

  void init();

  void reset();
  
  void resetPositionUniform();
  void resetPosition(double *pos, double *dev);
  
  void updatePositionMovement(const Motion::MotionLocalizationUpdate *move_info);
  void updatePositionSensors (int num_objects, const SensorInfo *sensor_info);
  // return true if at least one object seen
  bool updatePositionSensors (const VisionInterface::ObjectInfo *obj_info, const VisionInterface::RadialObjectMap *radial_map);
  
  void getPosition(RobotPositionInfo *pos_info);

  void copySamples(Sample *samples);

#ifdef PLATFORM_LINUX
  void dumpSamples(std::ostream &os) const;
#endif

protected:
  ulong last_motion_update_time;
  uint next_line_point_to_use;

  LocalizationEngine *engine;
  Environment *environ;
};

class SRLocalizerEP : public LocalizationEP {
private:
  uint mot_update_id;
  uint vis_id;

  MotionUpdateHeadSummary *mot_update_ep;
  Vision *vis_ep;

  EventCacheTracker cache_tracker;
  RobotPositionInfo *pos_info;

  SRLocalizer localizer;

public:
  static std::string name;

  static EventProcessor *create() {return new SRLocalizerEP;}

  SRLocalizerEP();
  virtual ~SRLocalizerEP();

  virtual bool initConnections();

  virtual bool update(ulong time,const EventList *events);

  virtual const RobotPositionInfo *get(ulong time);
};

#endif