/*!
  \example adjustWheelParameter.cpp 車輪系パラメータの調整サンプル

  \author Satofumi KAMIMURA

  $Id$

  \todo URG の測定エラーを処理すべき
  \todo URG の計測データが適切な値の範囲内かを比較すべき
*/

#include "mBeegoDrive.h"
#include "mUrgDevice.h"
#include "delay.h"
#include <iostream>

using namespace qrk;
using namespace std;


namespace
{
  double measureFront(RangeFinder& urg)
  {
    enum { CaptureTimes = 40 };

    int front_index = urg.deg2index(0);
    long total_mm = 0;
    size_t n = 0;
    for (size_t i = 0; i < CaptureTimes; ++i) {
      urg.requestData();

      vector<long> data;
      long timestamp;
      urg.receiveData(data, &timestamp);

      total_mm += data[front_index];
      ++n;
    }

    return 1.0 * total_mm / n;
  }
}


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

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

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

  // 前方の壁までの距離を測定する
  double first_mm = measureFront(urg);

  // 前方の壁の手前まで前進し、停止する
  enum {
    StopLength = 500,           // [mm]
  };
  run.followLine(0, 0, deg(0));
  int front_index = urg.deg2index(0);
  while (true) {
    urg.requestData();

    vector<long> data;
    urg.receiveData(data);

    if (data[front_index] < StopLength) {
      break;
    }
  }
  run.stop();
  while (! run.isStable()) {
    delay(100);
  }

  // 停止後の位置を計測し、車輪直径の設定比率を出力する
  double last_mm = measureFront(urg);
  Position<long> run_position = run.position();

  long run_length = run_position.x;
  double urg_length = first_mm - last_mm;

  cout << "run: " << run_length << " [mm]" << endl
       << "urg: " << urg_length << " [mm]" << endl;

  double adjust_ratio = 1.0 * urg_length / run_length;
  cout << "adjusted = current x " << adjust_ratio << endl;

  return 0;
}
