view/rangesCtrl/ScipHandler.cpp

/*!
  \file
  \brief SCIP ハンドラ

  \author Satofumi KAMIMURA

  $Id$

  \todo pImpl 内部に isConnected() を定義して利用する
*/

#include "ScipHandler.h"
#include "ConnectionInterface.h"
#include "SensorParameter.h"
#include "MathUtils.h"
#include "GetTicks.h"
#include "Delay.h"
#include <cstring>
#include <cstdlib>
#include <cstdio>

//#include "ConnectionLogger.h"

using namespace beego;


/*!
  \brief ScipHandler の内部クラス
*/
struct ScipHandler::pImpl {
  enum {
    TotalTimeout = 1000,
    Timeout = 800,

    //LineLength = 64 + 1 + 1 + 1,
    // !!! いずれ、以下を削除して、上の実装に戻すべき
    LineLength = 16 + 64 + 1 + 1 + 1, // 64 以上のデータが来る問題に対処

    RecvIncomplete = -1,
    PacketInvalid = -2,
  };

  std::string error_message;
  ConnectionInterface* con;
  SensorParameter params;
  int first, last;
  int data_byte;
  int groups;
  int skipFrames;
  int times;
  std::vector<long> recv_data;
  int remain_byte;
  char remain_data[3];
  int max_dataLength;
  bool laser_on;
  bool possible_remain;                //!< データの受信残りがある可能性があれば真

  pImpl(void)
    : error_message("no error."), con(NULL),
      first(0), last(0), data_byte(2), groups(1), skipFrames(0), times(1),
      remain_byte(0), max_dataLength(0), laser_on(false),
      possible_remain(true) {
  }

  static bool isLF(const char ch) {
    return ((ch == '\r') || (ch == '\n')) ? true : false;
  }

  long decode(const char* data, int data_byte) {
    long value = 0;
    for (int i = 0; i < data_byte; ++i) {
      value <<= 6;
      value &= ~0x3f;
      value |= data[i] - 0x30;
    }
    return value;
  }

  int readLine(char *buffer) {
    if (con == NULL) {
      return 0;
    }

    int i;
    for (i = 0; i < LineLength -1; ++i) {
      char recv_ch;
      int n = con->recv(&recv_ch, 1, pImpl::Timeout);
      if (n <= 0) {
        if (i == 0) {
          return -1;                // タイムアウト
        }
        break;
      }
      if (isLF(recv_ch)) {
        break;
      }
      buffer[i] = recv_ch;
    }

    buffer[i] = '\0';
    return i;
  }

  int sendTag(const char* tag) {
    if (! con) {
      return false;
    }

    char send_message[pImpl::LineLength];
    sprintf(send_message, "%s\n", tag);
    //fprintf(stderr, "send: %s, %d\n", tag, GetTicks());
    int send_size = strlen(send_message);
    int n = con->send(send_message, send_size);
    if (n < send_size) {
      // !!! error_message
      return -1;
    }
    return n;
  }

  int sendMessage(const char* tag, int timeout, int* recv_n = NULL) {
    if (recv_n) {
      *recv_n = -1;
    }
    if (! con) {
      return -1;
    }

    // 応答のみを返す
    int send_size = sendTag(tag);
    int recv_size = send_size + 2 + 1 + 2;
    char buffer[LineLength];

    int n = con->recv(buffer, recv_size, timeout);
#if 0
    if (! strcmp("QT", tag)) {
      //fprintf(stderr, "QT: n = %d\n[[[", n);
      for (int i = 0; i < n; ++i) {
        if (isprint(buffer[i])) {
          fprintf(stderr, "%c", buffer[i]);
        } else {
          fprintf(stderr, "[%02x]", (unsigned char)buffer[i]);
        }
      }
      fprintf(stderr, "]]]\n");
    }
#endif
    if (recv_n) {
      *recv_n = n;
    }
#if 0
    fprintf(stderr, "n = %d\n", n);
    for (int i = 0; i < n; ++i) {
      if (isprint(buffer[i])) {
        fprintf(stderr, "%c", buffer[i]);
      } else {
        fprintf(stderr, "[%02x]", (unsigned char)buffer[i]);
      }
    }
    fprintf(stderr, "\n");
#endif
    if (n < recv_size) {
      // !!! error_message
      error_message = "XX 1";
      return RecvIncomplete;
    }

    // tag がマッチするか確認
    if (strncmp(buffer, tag, send_size -1)) {
      // !!! error_message
      error_message = "XX 1.5";
      return PacketInvalid;
    }

    int data_length = recv_size - send_size;
    if (! checkSum(&buffer[send_size],
                   data_length - 3, buffer[send_size + data_length - 3])) {
      // !!! error_message
      error_message = "XX 2";
      return PacketInvalid;
    }

    // 応答の取得
    char reply_str[3] = "00";
    reply_str[0] = buffer[send_size];
    reply_str[1] = buffer[send_size + 1];
    int reply = strtol(reply_str, NULL, 16);

    return reply;
  }

  bool checkSum(const char* buffer, size_t size, char expected) {

    char sum = 0;
    for (size_t i = 0; i < size; ++i) {
      sum += buffer[i];
    }
    sum = (sum & 0x3f) + 0x30;

    return (sum == expected) ? true : false;
  }

  int addRecvData(const char buffer[]) {

    const char* pre_p = buffer;
    const char* p = pre_p;

    if (remain_byte > 0) {
      memmove(&remain_data[remain_byte], buffer, data_byte - remain_byte);
      recv_data.push_back(decode(remain_data, data_byte));
      pre_p = &buffer[data_byte - remain_byte];
      p = pre_p;

      remain_byte = 0;
    }

    do {
      ++p;
      if ((p - pre_p) >= static_cast<int>(data_byte)) {

        // !!! ここで、間引き数だけデータを増やすべき
        // !!! 未実装
        for (int i = 0; i < 1; ++i) {
          recv_data.push_back(decode(pre_p, data_byte));
        }
        pre_p = p;
      }
    } while (*p != '\0');
    remain_byte = p - pre_p;
    memmove(remain_data, pre_p, remain_byte);

    return 0;
  }

  void updateMaxDataLength(void) {
    max_dataLength = params.area_max +1;
  }

  int stopCapture(void) {

    // 停止させる必要がなければ、戻る
    if (! possible_remain) {
      return 0;
    }

    return forceStopCapture();
  }

  int forceStopCapture(bool no_wait_reply = false) {
    laser_on = false;

    if (no_wait_reply) {
      sendTag("QT");
      return 0;
    }

    int ret = sendMessage("QT", Timeout);
    if (ret > 0) {
      possible_remain = false;
    }
    return ret;
  }
};


ScipHandler::ScipHandler(void) : pimpl(new pImpl) {
}


ScipHandler::~ScipHandler(void) {
}


const char* ScipHandler::what(void) {
  return pimpl->error_message.c_str();
}


void ScipHandler::setConnection(ConnectionInterface* con) {
  pimpl->con = con;
  //pimpl->con = new ConnectionLogger(con, true);
}


ConnectionInterface* ScipHandler::getConnection(void) {
  return pimpl->con;
}


void ScipHandler::disconnect(void) {
  if (pimpl->con == NULL) {
    return;
  }

  // 受信の停止
  stopCapture();
  pimpl->con->disconnect();

  // 変数の初期化
  // !!! 関係なかった
  pimpl->remain_byte = 0;
}


bool ScipHandler::isConnected(void) const {

  return ((pimpl->con != NULL) && pimpl->con->isConnected()) ? true : false;
}


bool ScipHandler::adjustBaudrate(long baudrate) {

  // "SCIP2.0" を用いたボーレート合わせ
  long try_baudrate[] = { 19200, 115200, 57600, };
  for (size_t i = 0; i < sizeof(try_baudrate)/sizeof(try_baudrate[0]); ++i) {
    bool ret = pimpl->con->changeBaudrate(try_baudrate[i]);
    if (ret == false) {
      pimpl->error_message = pimpl->con->what();
      return false;
    }

    // 既存の取得コマンドの停止
    int reply = stopCapture();
    //fprintf(stderr, "reply: %d\n", reply);

    // SCIP2.0 コマンドを送信し、何かの応答('00' or '0e') で通信できたと見なす
    int recv_n = 0;
    if (reply != 0x00) {
      reply = pimpl->sendMessage("SCIP2.0", pImpl::Timeout, &recv_n);
      if ((recv_n < 0) && (reply < 0)) {
        // タイムアウト時は、ボーレートが違うと考えられるので再テストする
        continue;
      }
      if (reply == 0x0e) {
        // TMx モードの可能性もあるので、一応 TM2 を送信する
        pimpl->sendMessage("TM2", pImpl::Timeout);
      }
    }

    enum { SCIP1_REPLY_SIZE = 11, };
    if ((reply == 0x00) || (reply == 0x0e) || (recv_n == SCIP1_REPLY_SIZE)) {
      size_t before_ticks = GetTicks();
      ret = setBaudrate(baudrate);
      if (! ret) {
        // ボーレート変更に失敗したら、SS コマンドなし(USB 接続のみ)とみなす
        // その場合、通信成功とみなす
        return true;
      }
      int reply_msec = GetTicks() - before_ticks;

      if (ret) {
        // ホスト側のボーレートを変更する
        pimpl->con->changeBaudrate(baudrate);
        if ((reply == 0x00) || (recv_n == SCIP1_REPLY_SIZE)) {
          // シリアル通信の場合、ボーレート変更後、１周分だけ待つ必要がある
          delay(reply_msec *4/3 + 10);
        }
        return true;
      }
    }
  }

  pimpl->error_message = "Cannot adjust baudrate.";
  return false;
}


bool ScipHandler::setBaudrate(long baudrate) {

  // SS コマンドによるボーレート変更
  char buffer[] = "SS000000";
  sprintf(buffer, "SS%06ld", baudrate);
  int reply = pimpl->sendMessage(buffer, pImpl::Timeout);

  // Top-URG への暫定対応として、0xE でも true を返している
  // Top-URG 用の SCIP2.0 で SS がサポートされれば、0xE の条件は削除する
  return ((reply == 0) || (reply == 3) || (reply == 0xE)) ? true : false;
}


bool ScipHandler::getVersionInfo(std::vector<std::string>& lines) {

  pimpl->sendTag("VV");
  char buffer[pImpl::LineLength];
  int line_length;
  for (int i = 0; (line_length = pimpl->readLine(buffer)) > 0; ++i) {
    //fprintf(stderr, "%s\n", buffer);

    enum {
      TagReply = 0,
      DataReply,
      Other,
    };
    if (i == TagReply) {
      // !!!
      // !!! false

    } else if (i == DataReply) {
      // !!!
      // !!! false

    } else if (i >= Other) {
      buffer[line_length -2] = '\0';
      lines.push_back(&buffer[5]);
    }
  }
  return true;
}


bool ScipHandler::loadSensorParameter(SensorParameter* parameter) {

  // パラメータ読み出しと反映
  pimpl->sendTag("PP");
  char buffer[pImpl::LineLength];
  int line_index = 0;
  enum {
    TagReply = 0,
    DataReply,
    Other,
  };
  int line_length;
  for (; (line_length = pimpl->readLine(buffer)) > 0; ++line_index) {

    //fprintf(stderr, "%s\n", buffer);

    // チェックサムの確認
    // !!!
    // !!! return false;

    if (line_index == TagReply) {
      // !!!
      // !!! return false;

    } else if (line_index == DataReply) {
      // !!!
      // !!! return false;

    } else if (line_index == Other + SensorParameter::MODL) {
      buffer[line_length - 2] = '\0';
      pimpl->params.model = &buffer[5];

    } else if (line_index == Other + SensorParameter::DMIN) {
      pimpl->params.distance_min = atoi(&buffer[5]);

    } else if (line_index == Other + SensorParameter::DMAX) {
      pimpl->params.distance_max = atoi(&buffer[5]);
      if (pimpl->params.distance_max > 4095) {
        pimpl->data_byte = 3;
      }
    } else if (line_index == Other + SensorParameter::ARES) {
      pimpl->params.area_total = atoi(&buffer[5]);

    } else if (line_index == Other + SensorParameter::AMIN) {
      pimpl->params.area_min = atoi(&buffer[5]);
      pimpl->first = pimpl->params.area_min;

    } else if (line_index == Other + SensorParameter::AMAX) {
      pimpl->params.area_max = atoi(&buffer[5]);
      pimpl->last = pimpl->params.area_max;

    } else if (line_index == Other + SensorParameter::AFRT) {
      pimpl->params.area_front = atoi(&buffer[5]);

    } else if (line_index == Other + SensorParameter::SCAN) {
      pimpl->params.scan_rpm = atoi(&buffer[5]);
    }
  }

  if (line_index <= Other + SensorParameter::SCAN) {
    return false;
  }
  pimpl->updateMaxDataLength();

  if (parameter != NULL) {
    // 引数へのパラメータ代入
    *parameter = pimpl->params;
  }

  return true;
}


void ScipHandler::setSensorParameter(const SensorParameter* parameter) {
  pimpl->params = *parameter;
  pimpl->updateMaxDataLength();
}


void ScipHandler::setCaptureTimes(int times) {
  pimpl->times = times;
}


void ScipHandler::sendCaptureMessage(char cmd) {

  pimpl->remain_byte = 0;
  char send_message[pImpl::LineLength];
  if (cmd == 'M') {
    sprintf(send_message, "M%c%04d%04d%02d%01d%02d",
            (pimpl->data_byte == 2) ? 'S' : 'D',
            pimpl->first, pimpl->last,
            pimpl->groups, pimpl->skipFrames, pimpl->times);
    pimpl->possible_remain = true;

  } else {
    sprintf(send_message, "G%c%04d%04d%02d",
            (pimpl->data_byte == 2) ? 'S' : 'D',
            pimpl->first, pimpl->last,
            pimpl->groups);
  }

  // !!! 戻り値のチェックをすべき。一応
  pimpl->sendTag(send_message);
}


int ScipHandler::recvCaptureData(long* data, size_t max_size,
                                 size_t& timestamp) {

  pimpl->recv_data.clear();

  char message_type = 'M';
  char buffer[pImpl::LineLength];
  int line_length;
  size_t local_timestamp = 0;
  //fprintf(stderr, "%d, %d\n", pimpl->first, pimpl->last);
  for (int i = 0; (line_length = pimpl->readLine(buffer)) >= 0; ++i) {

    if (line_length < 0) {
      return -3;
    }

    //fprintf(stderr, "%s\n", buffer);

    // QT の応答をここでも監視する
    // MD 受信処理中の QT 送信は、QT と MD のどちらが来るかはわからないため
    if ((line_length >=3) && (! strncmp("QT", buffer, 2)) &&
        pImpl::isLF(buffer[2])) {
      // "00P\r\r" も読み出してしまう
      // !!! ひどいな！ 内部で読み出すこの仕組みは！
      // !!! 受信文字列長さが数値埋め込みだし
      int recv_n = pimpl->con->recv(buffer, 5, pImpl::Timeout);
      if (recv_n == 5) {
        return QT_Recv;
      }
    }

    // チェックサムの確認
    // !!!
    // !!! return false;

    if ((i >= 6) && (line_length == 0)) {

      // データ受信の完了
      for (size_t i = pimpl->recv_data.size(); i < max_size; ++i) {
        // データ末尾までを 19(計測有効範囲外)で埋める
        pimpl->recv_data.push_back(19);
      }
      for (size_t i = 0; i < max_size; ++i) {
        data[i] = pimpl->recv_data[i];
      }
      timestamp = local_timestamp;
      return pimpl->recv_data.size();

    } else if (i == 0) {
      // !!! ここで初期化するのは、どうだろう...。まぁ、効果あったが
      pimpl->remain_byte = 0;

      // 送信メッセージの最初の文字のみでメッセージの判定を行う
      // !!! 文字列すべてで、判定を行うべき
      if ((buffer[0] != 'M') && (buffer[0] != 'G')) {
        pimpl->error_message = "YY 01";
        //fprintf(stderr, "Packet invalid\n");
        return pImpl::PacketInvalid;
      }
      message_type = buffer[0];

      // 取得範囲を取り出す
      // !!! lexical_cast を用いるべきか？
      char first_str[] = "0000";
      // !!! ひどいな...。せめてコピーしましょうよ
      first_str[0] = buffer[2];
      first_str[1] = buffer[3];
      first_str[2] = buffer[4];
      first_str[3] = buffer[5];
      int first = atoi(first_str);

      // !!! この処理を、first 取り出し後に行えばよい
      // first または min までの領域を 19(計測有効範囲外)で埋める
      //fprintf(stderr, "first: %d, %d\n", pimpl->first, first);
      for (int i = first -1; i >= 0; --i) {
        pimpl->recv_data.push_back(19);
      }

    } else if (! strncmp(buffer, "99b", 3)) {
      // "99b" を検出し、以降を「タイムスタンプ」「データ」とみなす
      i = 4;

    } else if ((i == 1) && (message_type == 'G')) {
      i = 4;

      // !!! 'G' コマンド時の残り回数は、ここで取り出せる
      // !!! 取り出して、下記を有効にすべき
      // !!! 上記記述は、M の間違いじゃないのかな？
      if (0) {
        pimpl->laser_on = false;
        pimpl->possible_remain = false;
      }

    } else if (i == 2) {
      // 送信パケットに対する応答
      // !!!
      // 受信が終了したら、レーザーは停止する
      // !!! 終了を検出して、下記を有効にすべき
      if (0) {
        pimpl->laser_on = false;
      }

    } else if (i == 4) {
      // "99b" 固定
      if (strncmp(buffer, "99b", 3)) {
        pimpl->error_message = "YY 02";
        return pImpl::PacketInvalid;
      }

    } else if (i == 5) {
      // タイムスタンプ
      local_timestamp = pimpl->decode(buffer, 4);

    } else if (i >= 6) {
      // 取得データ
      if (line_length > (64 + 1)) {
        // !!! Top-URG のバグで 65 byte 以上のデータがくる問題に対処
        line_length = (64 + 1);
      }
      buffer[line_length -1] = '\0';
      int ret = pimpl->addRecvData(buffer);
      if (ret < 0) {
        pimpl->error_message = "YY 03";
        // !!! エラーメッセージの更新
        return ret;
      }
    }
  }
  return -3;
}


int ScipHandler::stopCapture(void) {
  int ret = pimpl->stopCapture();
  if (pimpl->con) {
    pimpl->con->skip(pImpl::TotalTimeout);
  }
  return ret;
}


void ScipHandler::forceStopCapture(bool no_wait_reply) {
  pimpl->forceStopCapture(no_wait_reply);
}


void ScipHandler::setFrameSkipFrames(size_t skip_frames) {
  if (skip_frames <= 9) {
    pimpl->skipFrames = skip_frames;
  }
}


void ScipHandler::setDataGroups(size_t groups) {
  if ((groups > 0) && (groups <= 99)) {
    pimpl->groups = groups;
  }
}


void ScipHandler::setCaptureRange(int first_index, int last_index) {

  if (first_index > last_index) {
    std::swap(first_index, last_index);
  }

  //fprintf(stderr, "%d, %d, %d\n", first_index, last_index, pimpl->params.area_max);

  if ((first_index >= 0) && (first_index <= pimpl->params.area_max)) {
    pimpl->first = first_index;
  } else {
    pimpl->first = 0;
  }
  if (last_index <= pimpl->params.area_max) {
    pimpl->last = last_index;
  } else {
    pimpl->last = pimpl->params.area_max;
  }
}


int ScipHandler::getFrontIndex(void) const {
  return (isConnected()) ? pimpl->params.area_front : -1;
}


long ScipHandler::getMinDistance(void) const {
  return (isConnected()) ? pimpl->params.distance_min : -1;
}


long ScipHandler::getMaxDistance(void) const {
  return (isConnected()) ? pimpl->params.distance_max : -1;
}


int ScipHandler::getMaxDataLength(void) const {

  if (! isConnected()) {
    // new long[0] だとエラーになるため、１を返す
    return 1;
  } else {
    return pimpl->max_dataLength;
  }
}


int ScipHandler::getScanRpm(void) {

  if (! isConnected()) {
    return -1;
  } else {
    return pimpl->params.scan_rpm;
  }
}


void ScipHandler::setLaserOutput(bool on) {
  if (pimpl->laser_on == on) {
    // 状態が変わらない場合は、戻る
    return;
  }

  int reply = pimpl->sendMessage("BM", pImpl::Timeout);

  if (reply >= 0) {
    pimpl->laser_on = on;
  }
}


double ScipHandler::index2rad(const int index) {
  if (! isConnected()) {
    return -1.0;
  }
  return (index - pimpl->params.area_front)
    * 2.0*M_PI / pimpl->params.area_total;
}


int ScipHandler::rad2index(const double radian) {
  if (! isConnected()) {
    return -1;
  }

  int index =
    static_cast<int>((radian * pimpl->params.area_total) / (2.0*M_PI))
    + pimpl->params.area_front;

  if (index < 0) {
    index = 0;
  } else if (index > pimpl->max_dataLength) {
    index = pimpl->max_dataLength;
  }
  return index;
}


size_t ScipHandler::getImmediateTimestamp(int* estimated_delay) {

  // 片道の通信遅延時間を推定して estimated_delay に格納して返す

  bool laser_state = pimpl->laser_on;
  int reply = pimpl->sendMessage("TM0", pImpl::Timeout);
  (void)reply;
  pimpl->laser_on = false;

  long last_timestamp = 0;
  pimpl->sendTag("TM1");

  // !!! 以下、いずれ作り直す。ひどすぎ...
  if (1) {
    char buffer[pImpl::LineLength];

#if 0
    int line_length = pimpl->readLine(buffer);
    line_length = pimpl->readLine(buffer);
    line_length = pimpl->readLine(buffer);
#endif
    last_timestamp = pimpl->decode(buffer, 4);
    pimpl->readLine(buffer);
  }

  size_t last_ticks = GetTicks();
  reply = pimpl->sendMessage("TM2", pImpl::Timeout);
  size_t spent_ticks = GetTicks();

  setLaserOutput(laser_state);

  return last_timestamp + (spent_ticks - last_ticks);
}