/*!
  \example adjustTreadParameter.cpp トレッド・パラメータの調整サンプル

  \author Satofumi KAMIMURA

  $Id$

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

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

using namespace qrk;
using namespace std;


namespace
{
  Angle measureAngle(RangeFinder& urg)
  {
    enum { CaptureTimes = 40 };

    // 前方の 30 deg の範囲を壁とみなし、その範囲のデータから傾きを計算する
    enum { DegreeRange = 30 };
    int first_index = urg.deg2index(- DegreeRange / 2);
    int last_index = urg.deg2index(+ DegreeRange / 2);

    double total_radian = 0.0;
    for (size_t i = 0; i < CaptureTimes; ++i) {

      urg.requestData();

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

      vector<Point<long> > points;
      for (int j = first_index; j < last_index; ++j) {

        double radian = urg.index2rad(j) - (M_PI / 2.0);
        Point<long> p = calculateXy<long>(data[j], radian);
        points.push_back(p);
      }
      double a, b;
      leastSquare<long>(a, b, &points[0], points.size());

      total_radian += atan2(a, 1.0);
    }

    return rad(total_radian / CaptureTimes);
  }
}


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;
  }

  // 前方の壁の傾きを計算する
  Angle first_angle = measureAngle(urg);

  // 360 x N 回だけ回転し、角度の差を求める
  enum {
    RotateTimes = 1,
  };
  run.spin(deg(360 * RotateTimes));
  while (! run.isStable()) {
    delay(100);
  }

  // 停止後の壁の傾きを計算し、角度の差を求める
  Angle last_angle = measureAngle(urg);

  cout << "first: " << first_angle.to_deg() << " [deg]" << endl
       << "last: " << last_angle.to_deg() << " [deg]" << endl;

  double radian_diff = last_angle.to_rad() - first_angle.to_rad();
  double adjust_ratio = (2.0 * M_PI) / ((2.0 * M_PI) + radian_diff);
  cout << "adjusted = current x " << adjust_ratio << endl;

  return 0;
}
