/*!
  \file
  \brief 3D マップの作成サンプル

  \author Satofumi KAMIMURA

  $Id: map3d.cpp 1364 2009-10-03 08:31:01Z satofumi $
*/

#include "qrk_main.h"
#include "mBeegoDrive.h"
#include "mUrgDevice.h"
#include "Position3d.h"
#include "convert3d.h"
#include "ticks.h"
#include "delay.h"
#include <fstream>
#include <iostream>

using namespace qrk;
using namespace std;


namespace
{
  void savePoints(ofstream& fout, const vector<Point3d<long> >& points)
  {
    for (vector<Point3d<long> >::const_iterator it = points.begin();
         it != points.end(); ++it) {
      fout << it->x << "\t" << it->y << "\t" << it->z << endl;
    }
  }
}


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

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

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

  // URG の取り付け角度
  const int urg_deg = 30;

  run.setStraightVelocity(200);
  run.followLine(Position<long>(0, 0, deg(0)));

  vector<long> data;

  const char filename[] = "points_3d.txt";
  ofstream fout(filename);
  if (! fout.is_open()) {
    perror(filename);
    return 1;
  }

  enum { MoveMsec = 5000 };
  int first_ticks = ticks();
  while ((ticks() - first_ticks) < MoveMsec) {

    // ロボット位置の取得
    Position3d<long> position;//  = run.position();
    (void)urg_deg; // !!! 反映させる

    urg.requestData();
    urg.receiveData(data);

    vector<Point3d<long> > points;
    convert3d(points, urg, data, position);

    // 3D 位置に変換した計測データを保存する
    savePoints(fout, points);
  }

  run.stop();

  return 0;
}
