/*!
  \example scip_20_cr.cpp

  \brief SS コマンドの実行サンプル

  \author Satofumi KAMIMURA

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

#include "SerialDevice.h"
#include "DetectOS.h"
#include "ConnectionUtils.h"
#include "ScipUtils.h"
#include "delay.h"

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

  // 回転周期を変えて、タイムスタンプ間隔を表示
  int percent[] = { 10, 5, 0 };
  size_t try_times = sizeof(percent)/sizeof(percent[0]);
  for (size_t i = 0; i < try_times; ++i) {

    printf("CR: %d\n", percent[i]);
    char command[] = "CRxx\n";
    sprintf(command, "CR%02d\n", percent[i]);
    con.send(command, 5);
    int ret = recvReply(&con, Timeout);
    if ((ret != 0x00) && (ret != 0x0e) && (ret != 0x03)) {
      printf("CR: %d\n", ret);
      exit(1);
    }

    // 少し待つ
    delay(500);

    // MD で 1 step 分のデータを取得。4 行目がタイムスタンプなのを利用する
    con.send("MD0000000101005\n", 16);
    // !!! 受信データが帰ってくるのを待つ
    delay(Timeout);

    ret = recvReply(&con, Timeout);
    if (ret != 0x00) {
      printf("MD: %d\n", ret);
      exit(1);
    }

    int pre_timestamp = 0;
    for (int i = 0; i < 5; ++i) {
      // 受信データの表示
      size_t line_count = 0;
      enum { LineMax = 64 + 1 };
      char buffer[LineMax];
      int n;
      while ((n = readline(&con, buffer, LineMax, Timeout)) > 0) {
        if (line_count == 2) {
          int timestamp = decode(buffer, 4);
          int diff = timestamp - pre_timestamp;
          if (diff < Timeout) {
            // タイムアウトよりも短ければ、正常な間隔とみなして出力
            printf("%d [msec]\n", diff);
          }
          pre_timestamp = timestamp;
        }
        ++line_count;
      }
    }
  }
  return 0;
}
