/*!
  \example evaluateGdTimestamp.cpp

  \brief GD データ取得におけるタイムスタンプの評価プログラム

  \author Satofumi KAMIMURA

  $Id: evaluateGdTimestamp.cpp 1053 2009-06-20 22:52:51Z satofumi $

  \todo evaluateTmTimestamp との共通関数をくくる
*/

#include "SerialDevice.h"
#include "ConnectionUtils.h"
#include "ticks.h"
#include "delay.h"
#include <SDL.h>

using namespace qrk;

namespace
{
  enum {
    Timeout = 1000,
    BufferSize = 255,
  };
}


static void skipLines(SerialDevice& con, int lines)
{
  char buffer[BufferSize];
  for (int i = 0; i < lines; ++i) {
    if (readline(&con, buffer, BufferSize, Timeout) <= 0) {
      break;
    }
  }
}


static int encode(const char code[], int byte)
{
  int value = 0;
  for (int i = 0; i < byte; ++i) {
    value <<= 6;
    value &= ~0x3f;
    value |= code[i] - 0x30;
  }
  return value;
}


static int getUrgTimestamp(SerialDevice& con)
{
  con.send("GD0000000101\r", 13);

  // エコーバックと戻り値を読み飛ばす
  skipLines(con, 2);

  char buffer[BufferSize];
  if (readline(&con, buffer, BufferSize, Timeout) <= 0) {
    fprintf(stderr, "timeout\n");
    return -1;
  }
  int timestamp = encode(buffer, 4);

  // 受信データを読み飛ばす
  skipLines(con, 2);

  return timestamp;
}


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

  SerialDevice con;
  if (! con.connect("/dev/ttyACM0", 19200)) {
    printf("SerialDevice::connect: %s\n", con.what());
    exit(1);
  }
  con.send("SCIP2.0\r", 8);
  skip(&con, Timeout);

  con.send("BM\r", 3);
  skip(&con, Timeout);

  enum {
    EvaluateTimes = 60 * 10,
  };

  int first_urg_timestamp = getUrgTimestamp(con);
  int first_pc_timestamp = ticks();

  for (int i = 0; i < EvaluateTimes; ++i) {
    int urg_timestamp = getUrgTimestamp(con);
    int pc_timestamp = ticks();
    printf("%d\t%d\n",
           pc_timestamp - first_pc_timestamp,
           urg_timestamp - first_urg_timestamp);
    fprintf(stderr, "%d\t%d\n",
            pc_timestamp - first_pc_timestamp,
            urg_timestamp - first_urg_timestamp);
    delay(1000);
  }

  return 0;
}
