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

#include <stdlib.h>
#include <stdio.h>

#include <FL/Fl_Window.H>

#include "../../agent/Vision/colors.h"
#include "../../agent/Vision/cmv_types.h"
#include "../../agent/Vision/Vision.h"
#include "../../agent/WorldModel/LocalModel.h"
#include "../log_processing/shared/LogReader.h"
#include "../log_processing/shared/VisionRLEPacketDecoder.h"

class AppWindow;

class Canvas {
public:
  int w,h;
  vector2f minv,maxv;

  // functions for going from world coordinates to screen coordinates
  int transform_x(float x);
  int transform_y(float y);
  int scale_x(float x);
  int scale_y(float y);
  vector2i operator()(vector2f world_coor);

  // functions for going from screen coordinates to world coordinates
  float reverse_transform_x(int x);
  float reverse_transform_y(int y);
  vector2f reverse_transform(vector2i screen_coor);

public:
  void set_size(int _w,int _h);
  void set_range(vector2f _minv,vector2f _maxv);

  void point(float xc,float yc);
  void dot(float xc,float yc,int d);
  void line(float x1,float y1,float x2,float y2);
  // x1 <= x2 and y1 <= y2
  void rectf(float x1,float y1,float x2,float y2);
  void rectf_wh(float x,float y,float w,float h);
  void circlef(float xc,float yc,float r);
};

class DisplayProcessor {
public:
  Canvas *canvas;
  ulong min_time;
  ulong cur_time;

  void add_loc(LocalModel::MapPoint *pt,vector2f loc);
};

class LocalModelTester {
private:
  static const int MaxImageXSize = 208;
  static const int MaxImageYSize = 160;

  static const int MaxPacketLength=120*1024;
  RobotDataPacket packet;
  MotionPacketDecoder mot_decoder;
  VisionPacketDecoder vis_decoder;
  VisionRLEPacketDecoder vis_rle_decoder;

  uchar *img;
  Vision vision;

  int imageXSize;
  int imageYSize;

  LocalModel::LModel model;

  ulong cur_time;
  bool draw_occ_grid;
  bool draw_points;
  bool draw_separators;

public:
  LogReader log_reader;

  LocalModelTester();
  ~LocalModelTester();

  void init();

  void run_vision(RadialObjectMap *rad_map,double *angles,uchar *img);

  void run_next_packet(bool &did_something,bool &out_of_data);

  void toggle_draw_points()   {draw_points   = !draw_points;}
  void toggle_draw_occ_grid() {draw_occ_grid = !draw_occ_grid;}
  void draw_data(AppWindow *win,Canvas &canvas);

  void run_debug_xy(float x,float y);
};

class AppWindow : public Fl_Double_Window {

 private:
  bool keep_running;

  char *log_filename;
  FILE *log_file;
  LocalModelTester tester;

  Canvas canvas;

  /// Print the on-line key help 
  void printKeyHelp();

 public:

  /// Constructor
  AppWindow();

  /// Destructor
  ~AppWindow();

  /// Calculate and advance the frame
  void advanceFrame();

  /// Save the current frame
  void saveFrame(const char * fn);

  /// Set the log file
  void set_log_file(const char *_log_filename);

  /// Resize the window
  virtual void resize(int x,int y,int w,int h);

  /// Override the default draw method
  virtual void draw();

  /// Handle events
  virtual int handle(int event);

  /// Run the FLTK event loop 
  void run();
};


#endif