/*!
  \example convert2dSample.cpp

  \brief X-Y 平面への距離データ変換サンプル

  \author Satofumi KAMIMURA

  $Id: convert2dSample.cpp 1739 2010-03-06 07:43:32Z satofumi $
*/

#include "mUrgDevice.h"
#include <SDL.h>

using namespace qrk;
using namespace std;


int main(int argc, char *argv[])
{
  mUrgDevice urg(argc, argv);
  if (! urg.connect("/dev/ttyACM0")) {
    printf("UrgDevice::connect: %s\n", urg.what());
    exit(1);
  }

  int min_length = urg.minDistance();
  int max_length = urg.maxDistance();

  vector<long> data;
  int n = urg.capture(data);

  for (int i = 0; i < n; ++i) {
    long length = data[i];
    if ((length <= min_length) || (length >= max_length)) {
      continue;
    }

    double radian = urg.index2rad(i);
    int x = static_cast<int>(length * cos(radian));
    int y = static_cast<int>(length * sin(radian));

    printf("%d\t%d\t#%d\n", x, y, i);
  }

  return 0;
}
