/*!
  \example evaluateMdTimestamp.cpp

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

  \author Satofumi KAMIMURA

  $Id: evaluateMdTimestamp.cpp 1939 2010-11-22 02:05:24Z 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)
{
  // エコーバックと戻り値を読み飛ばす
  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[]) {

  const char *device = "/dev/ttyACM0";
  if (argc > 1) {
    device = argv[1];
  }

  SerialDevice con;
  if (! con.connect(device, 19200)) {
    printf("SerialDevice::connect: %s\n", con.what());
    exit(1);
  }
  con.send("QT\r", 3);
  con.send("TM2\r", 4);
  con.send("SCIP2.0\r", 8);
  skip(&con, Timeout);

  enum {
    EvaluateTimes = 60 * 100 * 10,
  };

  con.send("MD0000000101000\r", 16);

  // エコーバックの読み捨て
  getUrgTimestamp(con);
  getUrgTimestamp(con);

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

  int pre_pc_timestamp = ticks();
  for (int i = 0; i < EvaluateTimes; ++i) {
    int urg_timestamp = getUrgTimestamp(con);
    int pc_timestamp = ticks();

    if ((pc_timestamp - pre_pc_timestamp) >= 0) {
        pre_pc_timestamp = pc_timestamp;
        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);
    }
  }

  return 0;
}
