/*!
  \example scip_20_tm.cpp

  \brief SCIP 2.0 の "TMx" 実行サンプル

  \author Satofumi KAMIMURA

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

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

using namespace qrk;


static long recvTm1Reply(Connection* con, int timeout) {

  con->send("TM1\n", 4);

  // エコーバック、応答の読み出し
  int ret = recvReply(con, timeout);
  if (ret != 0x00) {
    fprintf(stderr, "recvReply: %d\n", ret);
    return -1;
  }

  // タイムスタンプの読み出し
  enum { TimestampLine = 5 + 1 };
  char buffer[TimestampLine];
  int n = readline(con, buffer, TimestampLine, timeout);
  long timestamp = 0;
  if (n == 5) {
    // タイムスタンプを示す文字列を出力する
    printf("%s\n", buffer);

    // タイムスタンプのデコード
    timestamp = decode(buffer, 4);
  }

  // 最後の改行の読み出し
  char recv_ch = '\0';
  n = con->receive(&recv_ch, 1, timeout);
  if (! isLF(recv_ch)) {
    return -2;
  }

  return timestamp;
}


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

  // TM0 により、タイムスタンプモードへ遷移する
  con.send("TM0\n", 4);
  int ret = recvReply(&con, Timeout);
  if (ret != 0x00) {
    printf("%d: Please reboot URG.\n", ret);
    exit(1);
  }

  enum { TryTimes = 3 };
  for (int i = 0; i < TryTimes; ++i) {
    // TM1 で、タイムスタンプを取得し、表示する
    long timestamp = recvTm1Reply(&con, Timeout);
    printf("=> %ld [msec]\n\n", timestamp);
    delay(1);
  }

  // TM2 で、タイムスタンプモードから抜ける
  con.send("TM2\n", 4);
  ret = recvReply(&con, Timeout);
  if (ret != 0x00) {
    printf("%d: Please reboot URG.\n", ret);
    exit(1);
  }

  printf("exit normaly.\n");

  return 0;
}
