/*!
  \example evaluateTmTimestamp.cpp

  \brief TM モードにおけるタイムスタンプの評価プログラム

  \author Satofumi KAMIMURA

  $Id: evaluateTmTimestamp.cpp 1906 2010-09-13 01:52:15Z satofumi $
*/

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

using namespace qrk;


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;
    }
    //fprintf(stderr, "%d: %s\n", i, buffer);
  }
}


static void beginTmMode(SerialDevice& con)
{
  con.send("TM0\r", 4);
  skipLines(con, 3);
}


static void endTmMode(SerialDevice& con)
{
  con.send("TM2\r", 4);
  skipLines(con, 3);
}


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("TM1\r", 4);
  skipLines(con, 2);

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

  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("QT\r", 3);
  con.send("TM2\r", 4);
  con.send("SCIP2.0\r", 8);
  skip(&con, Timeout);

  beginTmMode(con);

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

  endTmMode(con);

  return 0;
}
