mURGCtrl.cpp

/*
  モニタ対応の URG 制御クラス
  Satofumi KAMIMURA
  $Id$
*/

#include "mURGCtrl.h"
#include "parseArgs.h"
#include <string.h>


#ifndef PACKAGE_STR_VERSION
#define PACKAGE_STR_VERSION "0.0.0"
#endif


URG_Simulator* mURGCtrl::simulator = NULL;
int mURGCtrl::sim_add_count = 0;
bool mURGCtrl::isVersionPrinted = false;
bool mURGCtrl::isHelpPrinted = false;


mURGCtrl::mURGCtrl(void)
  : cond(SDL_CreateCond()), mutex(SDL_CreateMutex()),
    mon(vmonitor::getObject()), MonitorMode(Monitor::Unknown),
    pre_captureTimes(0), pre_ret_value(0),
    playback_isconnected(false), no_record(false) {
}


mURGCtrl::~mURGCtrl(void) {

  if (isConnected()) {
    --sim_add_count;
    if (sim_add_count == 0) {
      mon->del(simulator);
      delete simulator;
    }
  }
  SDL_DestroyMutex(mutex);
  SDL_DestroyCond(cond);
}


void mURGCtrl::printVersion(void) {
  printf("URG Ctrl Library with Monitor " PACKAGE_STR_VERSION "\n");
}


void mURGCtrl::printHelp(void) {
  printf("\n"
         "----- URG Ctrl Library with Monitor -----\n"
         "Options:\n"
         "None\n"
         "\n");
}


void mURGCtrl::parseArgs(int argc, char* argv[]) {
  bool help = false;
  bool version = false;

  for (int i = 0; i < argc; ++i) {
    if (!strcmp("--help", argv[i]) || !strcmp("-h", argv[i])) {
      help = true;

    } else if (!strcmp("--version", argv[i]) || !strcmp("-v", argv[i])) {
      version = true;
    }
  }
  if (version && !isVersionPrinted) {
    printVersion();
    isVersionPrinted = true;
  }
  if (help && !isHelpPrinted) {
    printHelp();
    isHelpPrinted = true;
  }
}


bool mURGCtrl::isConnected(void) {
  if (MonitorMode == Monitor::Playback) {
    return playback_isconnected;
  } else {
    return URGCtrl::isConnected();
  }
}


int mURGCtrl::connect(bool autoCapture, const char* device, long baudrate) {
  if (MonitorMode == Monitor::Playback) {
    // バージョン情報の読み出し
    checkVersion();
    if (enableTimestamp) {
      initTicksInfo();
    }
    return 0;
  }
  return URGCtrl::connect(device, baudrate, autoCapture);
}


int mURGCtrl::connect(bool autoCapture) {
  if (MonitorMode == Monitor::Playback) {
    //to_simulator = false;
  }
  return URGCtrl::connect(autoCapture);
}


int mURGCtrl::connect(int argc, char *argv[], bool autoCapture) {

  MonitorMode = mon->connect(argc, argv);
  switch (MonitorMode) {
  case Monitor::Playback:
    playback_isconnected = true;
    to_simulator = true;

  case Monitor::Simulator:
    if (!simulator) {
      simulator = new URG_Simulator();
      simulator->setURGType(mon->getURGType());
      simulator->setEnvironment(mon->env->getPolygonsReference());
    }
  }
  // !!! connect() を呼んで接続が行われなかったら破綻するはず。~ 込みで
  if (sim_add_count <= 0) {
    if (simulator) {
      simulator->setURGObject(this);
    }
    mon->add(simulator);
  }
  ++sim_add_count;

  parseArgs(argc-1, &argv[1]);
  int ret = URGCtrl::connect(argc, argv, autoCapture);
  if (MonitorMode == Monitor::Playback) {
    mon->del(simulator);
    --sim_add_count;
  }
  return ret;
}


void mURGCtrl::writeLogData(int ret_value, unsigned long ticks,
                            unsigned long raw_timestamp) {
  int captureTimes = URGCtrl::getCaptureTimes();
  mon->log->lock();
  mon->log->writeTag("urg", "capture", ticks);
  fprintf(mon->log->fd, " times=%d", captureTimes);

  if (captureTimes != pre_captureTimes) {
    fprintf(mon->log->fd, " ret=%d", ret_value);
    if (enableTimestamp) {
      fprintf(mon->log->fd, " raw_timestamp=%lu", raw_timestamp);
    }
    if (ret_value >= 0) {
      fprintf(mon->log->fd, " data=\"%ld", length[0]);
      for (int i = 1; i < ret_value; ++i) {
        fprintf(mon->log->fd, ",%ld", length[i]);
      }
      fprintf(mon->log->fd, "\"");
    }
  }
  mon->log->writeTagEnd();
  mon->log->unlock();
  pre_captureTimes = captureTimes;
}


void mURGCtrl::readLogData(int *ret_value, unsigned long* raw_timestamp) {
  mon->log->lock();
  unsigned long ticks = mon->log->readTag("urg", "capture");

  const char* line = mon->log->getLineBuffer();
  char data_buf[6000];
  int times = 0;
  int ret = 0;

  if (enableTimestamp) {
    sscanf(line, "%*s %*s times=%d ret=%d raw_timestamp=%lu %s",
           &times, &ret, raw_timestamp, data_buf);
  } else {
    sscanf(line, "%*s %*s times=%d ret=%d %s", &times, &ret, data_buf);
  }

  if (times == pre_captureTimes) {
    // 前回と同じデータを返す
    memcpy(length, pre_length, sizeof(length[0]) * SenseSteps);
    *ret_value = pre_ret_value;

  } else {
    // センサ値をログから取得
    *ret_value = ret;
    pre_ret_value = *ret_value;

    char *p = data_buf + strlen("data=\"");
    for (int i = 0; i < ret; ++i) {
      sscanf(p, "%ld", &length[i]);
      p = strchr(p, ',') + 1;
    }
    memcpy(pre_length, length, sizeof(length[0]) * SenseSteps);
    pre_captureTimes = times;
  }
  mon->log->unlock();
  mon->task->waitToTicks(ticks, cond, mutex);
}


void mURGCtrl::writeVersionInfo(int ret_value, char lines[][LineLength],
                                unsigned long ticks) {
  mon->log->lock();
  mon->log->writeTag("urg", "getVersionInfo", ticks);
  fprintf(mon->log->fd, " ret=%d", ret_value);
  if (ret_value >= 0) {
    fprintf(mon->log->fd, " \"%s;%s;%s;%s;%s;%s;%s;\"",
            lines[0], lines[1], lines[2],
            lines[3], lines[4], lines[5], lines[6]);
  }
  mon->log->writeTagEnd();
  mon->log->unlock();
}


void mURGCtrl::readVersionInfo(int* ret_value, char lines[][LineLength]) {
  mon->log->lock();
  unsigned long ticks = mon->log->readTag("urg", "getVersionInfo");

  const char* line = mon->log->getLineBuffer();
  sscanf(line, "%*s %*s ret=%d", ret_value);
  if (*ret_value >= 0) {
    const char* begin = strchr(line, '\"') +1;
    for (int i = 0; i < 7; ++i) {
      const char *end = strchr(begin, ';');
      memcpy(lines[i], begin, end - begin);
      lines[i][end - begin] = '\0';
      begin = end + 1;
    }
  }
  mon->log->unlock();

  mon->task->waitToTicks(ticks, cond, mutex);
}


int mURGCtrl::getVersionInfo(char lines[][LineLength]) {
  int ret_value = -1;
  if (MonitorMode == Monitor::Playback) {
    // ログからの読み出し
    readVersionInfo(&ret_value, lines);

  } else {
    // ログへの書き込み
    ret_value = URGCtrl::getVersionInfo(lines);
    writeVersionInfo(ret_value, lines, mon->getTicks());
  }
  return ret_value;
}


int mURGCtrl::raw_capture(long length[],
                          int first_index, int last_index, int group,
                          unsigned long* raw_timestamp) {
  int ret_value = -1;
  if (MonitorMode == Monitor::Playback) {
    // ログからの読み出し
    readLogData(&ret_value, raw_timestamp);

  } else {
    // ログへの書き込み
    if (raw_timestamp != NULL) {
      ret_value = URGCtrl::raw_capture(length, first_index, last_index, group,
                                       raw_timestamp);
      writeLogData(ret_value, mon->getTicks(), *raw_timestamp);
    } else {
      ret_value = URGCtrl::raw_capture(length, first_index, last_index, group);
      writeLogData(ret_value, mon->getTicks(), 0);
    }
  }
  return ret_value;
}


int mURGCtrl::getCaptureTimes(void) {
  int ret_times = 0;

  mon->log->lock();
  if (MonitorMode == Monitor::Playback) {
    // ログからの読み出し
    unsigned long ticks = mon->log->readTag("urg", "getCaptureTimes");
    const char* line = mon->log->getLineBuffer();
    sscanf(line, "%*s %*s ret=%d", &ret_times);
    mon->log->unlock();
    mon->task->waitToTicks(ticks, cond, mutex);

  } else {
    // ログへの書き込み
    ret_times = URGCtrl::getCaptureTimes();
    mon->log->writeTag("urg", "getCaptureTimes", mon->getTicks());
    fprintf(mon->log->fd, " ret=%d", ret_times);
    mon->log->writeTagEnd();
    mon->log->unlock();
  }
  return ret_times;
}


unsigned long mURGCtrl::getHostTicks(void) {
  unsigned long host_ticks = 0;

  mon->log->lock();
  if (MonitorMode == Monitor::Playback) {
    // ログからの読み出し
    unsigned long ticks = mon->log->readTag("urg", "getHostTicks");
    const char* line = mon->log->getLineBuffer();
    sscanf(line, "%*s %*s ret=%lu", &host_ticks);
    mon->log->unlock();
    mon->task->waitToTicks(ticks, cond, mutex);

  } else {
    // ログへの書き込み
    host_ticks = URGCtrl::getHostTicks();
    mon->log->writeTag("urg", "getHostTicks", mon->getTicks());
    fprintf(mon->log->fd, " ret=%lu", host_ticks);
    mon->log->writeTagEnd();
    mon->log->unlock();
  }
  return host_ticks;
}


void mURGCtrl::beginTimeAdjust(void) {
  if (MonitorMode != Monitor::Playback) {
    URGCtrl::beginTimeAdjust();
  }
}


void mURGCtrl::endTimeAdjust(void) {
  if (MonitorMode != Monitor::Playback) {
    URGCtrl::endTimeAdjust();
  }
}


unsigned long mURGCtrl::getURGTimestamp(void) {
  unsigned long raw_ticks = 0;

  mon->log->lock();
  if (MonitorMode == Monitor::Playback) {
    // ログからの読み出し
    unsigned long ticks = mon->log->readTag("urg", "getURGTimestamp");
    const char* line = mon->log->getLineBuffer();
    sscanf(line, "%*s %*s ret=%lu", &raw_ticks);
    mon->log->unlock();
    mon->task->waitToTicks(ticks, cond, mutex);

  } else {
    // ログへの書き込み
    raw_ticks = URGCtrl::getURGTimestamp();
    mon->log->writeTag("urg", "getURGTimestamp", mon->getTicks());
    fprintf(mon->log->fd, " ret=%lu", raw_ticks);
    mon->log->writeTagEnd();
    mon->log->unlock();
  }
  return raw_ticks;
}