/*!
  \example scip_20_gd.cpp

  \brief GD コマンドによるデータ取得

  \author Satofumi KAMIMURA

  $Id: scip_20_gd.cpp 1099 2009-06-30 02:47:18Z satofumi $
*/

#include "DetectOS.h"
#include "SerialDevice.h"
#include "ConnectionUtils.h"
#include "ScipUtils.h"
#include <cstring>

using namespace qrk;


//! main
int main(int argc, char *argv[]) {

  // 適切にポート名を変更すること
#if defined(WINDOWS_OS)
  const char device[] = "COM3";
#elif defined(LINUX_OS)
  const char device[] = "/dev/ttyACM0";
#else
  const char device[] = "/dev/tty.usbmodem1d11";
#endif

  SerialDevice con;
  if (! con.connect(device, 19200)) {
    printf("SerialDevice::connect: %s\n", con.what());
    exit(1);
  }

  // SCIP2.0 の呼び出しと、応答の無視
  enum { Timeout = 200 };
  con.send("SCIP2.0\n", 8);
  skip(&con, Timeout);

  // "BM" で URG を点灯させる
  con.send("BM\n", 3);
  int ret = recvReply(&con, Timeout);
  if (ret < 0) {
    printf("BM fail: %d\n", ret);
    exit(1);
  }

  // "GD" コマンドの送信
  /*
    GD    または、GS
    0000  AMIN の値を使うべき
    0768  AMAX 以下でなければならない
    01    まとめる数
    まとめる数を X にすると、
    連続する X 個の距離データのうち最小の距離データが返される
    受信データを減少させることができる
  */
  con.send("GD0000076801\n", 13);

  // 受信データの表示
  enum {
    LineMax = 64 + 1 + 1,
    URG_04LX_MAX = 768,
    PacketByte = 3,             // GD なので、3byte
  };
  char buffer[LineMax];
  char data_buffer[(URG_04LX_MAX + 1) * PacketByte];
  int filled = 0;
  int line_count = 0;
  int n;
  while ((n = readline(&con, buffer, LineMax, Timeout)) > 0) {
    printf("%s\n", buffer);

    // デコード用にデータを保管 (３行目以降が、距離データ)
    // line_count 0: エコーバック
    //            1: ステータス
    //            2: タイムスタンプ
    if (line_count >= 3) {
      int copy_size = n - 1;   // 最後のチェックサムは除外
      memmove(&data_buffer[filled], buffer, copy_size);
      filled += copy_size;
    }
    ++line_count;
  }
  printf("\n");

  // 受信データのデコード
  // 可能ならば、行毎にデータのデコード処理を行ったほうが、メモリにやさしい
  for (int i = 0; i < URG_04LX_MAX; ++i) {
    // 有効な距離データは、[DMIN, DMAX] のみであることに注意
    int length = decode(&data_buffer[i * PacketByte], PacketByte);
    printf("%d, ", length);
    if ((i % 10) == 00) {
      printf("\n");
    }
  }
  printf("\n");

  return 0;
}
