/*!
  \file
  \brief URG を用いた向きの表示サンプル

  URG を水平に置き、プログラムを実行した状態で回転させたりして動作を確認する

  \author Satofumi KAMIMURA
*/

#include "LrfDirection.h"
#include "UrgDevice.h"
#include <iostream>

using namespace qrk;
using namespace std;


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

  const char device[] = "/dev/ttyACM0";

  UrgDevice urg;
  if (! urg.connect(device)) {
    cout << "UrgDevice::connect(): " << urg.what() << endl;
    return 1;
  }

  LrfDirection urg_direction(&urg);

  // urg_direction.setDirection(deg(0));

  vector<long> data;
  while (true) {
    urg.requestData();
    if (urg.receiveData(data, NULL)) {
      urg_direction.update(data);
    }

    // 向きの表示
    double degree = urg_direction.direction().to_rad() / M_PI * 180.0;
    cout << degree << " [deg]" << endl;
  }
}
