/*!
  \file
  \brief マップ画像の生成プログラム

  \author Satofumi KAMIMURA

  $Id: map_generator.cpp 1536 2009-11-19 01:21:26Z satofumi $
*/

#include "grid_width.h"
#include "qrk_main.h"
#include "mBeegoDrive.h"
#include "mUrgDevice.h"
#include "convert2d.h"
#include "PixelMap.h"
#include "Color.h"
#include <iostream>

using namespace qrk;
using namespace std;


namespace
{
  enum {
    RobotPath = 64,
  };
}


int main(int argc, char *argv[])
{
  static_cast<void>(argc);
  static_cast<void>(argv);

  mBeegoDrive run;
  if (! run.connect("/dev/ttyUSB0")) {
    cout << "BeegoDrive::connect: " << run.what() << endl;
    return 1;
  }

  mUrgDevice urg;
  if (! urg.connect("/dev/ttyACM0")) {
    cout << "UrgDevice::connect: " << urg.what() << endl;
    return 1;
  }
  urg.setSkipLines(2);

  Position<long> position;
  vector<long> data;

  Grid run_path;
  Grid urg_points;
  Point<long> min_point;
  Point<long> max_point;
  int max_score = 0;

  // !!! 最初の位置の向きを記録してマップ生成で利用すべき

  try {
    while (true) {
      // URG データの記録
      urg.requestData();
      long timestamp = 0;
      urg.receiveData(data, &timestamp);
      fprintf(stderr, "%ld, ", timestamp / 1000);

      // 走行コントローラの位置記録
      Position<long> run_position = run.position();

      // 走行軌跡を記録
      int path_y = run_position.y / GridWidth;
      int path_x = run_position.x / GridWidth;
      run_path[path_y][path_x] = RobotPath;

      // URG データを記録
      vector<Point<long> > points;
      convert2d(points, urg, data, run_position);

      for (vector<Point<long> >::const_iterator it = points.begin();
           it != points.end(); ++it) {

        // 画像サイズの保持
        min_point.x = min(min_point.x, it->x);
        min_point.y = min(min_point.y, it->y);
        max_point.x = max(max_point.x, it->x);
        max_point.y = max(max_point.y, it->y);

        // 記録
        long x = it->x / GridWidth;
        long y = it->y / GridWidth;
        max_score = max(max_score, ++urg_points[y][x]);
      }
    }
  } catch (std::exception& e) {
  }

  long width = (max_point.x - min_point.x) / GridWidth;
  long height = (max_point.y - min_point.y) / GridWidth;
  PixelMap pixel_map(Rect<long>(min_point.x / GridWidth,
                                min_point.y / GridWidth,
                                width, height));

  Color white(1.0, 1.0, 1.0);
  pixel_map.addPoints(run_path, white, 256);

  Color red(1.0, 0.0, 0.0);
  pixel_map.addPoints(urg_points, red, max_score);

  pixel_map.generate();

  return 0;
}
