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

#include <FL/Fl_Window.H>

#include "headers/MessageStructures.hh"
#include "Localization/SRL/Localization.h"
#include "Motion/MotionInterface.h"
#include "Vision/Vision.h"
#include "Vision/VisionInterface.h"

#include "log_processing/shared/LogReader.h"
#include "log_processing/shared/PacketDecoder.h"
#include "log_processing/shared/MotionPacketDecoder.h"
#include "log_processing/shared/VisionPacketDecoder.h"
#include "log_processing/shared/VisionRLEPacketDecoder.h"

class AppWindow;
class Canvas;

class LocalizationTester {
public:
  static const int DefImageXSize = 176;
  static const int DefImageYSize = 144;
  static const int MaxImageXSize = 208;
  static const int MaxImageYSize = 160;

  static const int MaxPacketLength=120*1024;

private:
  SRLocalizer localizer;

  Motion::MotionLocalizationUpdate motion_update;
  VisionInterface::ObjectInfo obj_info;
  RobotPositionInfo pos_info;

  Vision vision;
  CMVision::image_classified vis_img;

  RobotDataPacket packet;
  LogReader log_reader;
  FILE *log_file;
  VisionPacketDecoder vision_decoder;
  VisionRLEPacketDecoder vision_rle_decoder;
  MotionPacketDecoder motion_decoder;
  int dist_cnt;
  int move_cnt;
  int vis_cnt;

  void draw_obj_info(AppWindow *win,Canvas &canvas,vector2f robot_loc,vector2f robot_dir);
  void draw_point_info(AppWindow *win,Canvas &canvas,vector2f robot_loc,vector2f robot_dir);

public:
  LocalizationTester();
  ~LocalizationTester();

  void init(const char *log_filename);

  bool step(int step_cnt);
  void reset();

  void draw_data(AppWindow *win,Canvas &canvas);
};

#endif