/*!
  \file
  \brief SCIP プロトコル処理

  \author Satofumi KAMIMURA

  $Id: ScipHandler.cpp 1751 2010-03-23 18:56:02Z satofumi $
*/

#include "ScipHandler.h"
#include "RangeFinderParameter.h"
#include "Connection.h"
#include "ConnectionUtils.h"
#include "ticks.h"
#include "log_printf.h"
#include "delay.h"
#include <cstring>
#include <cstdio>

using namespace qrk;
using namespace std;


namespace
{
  /*!
    \brief 受信データのタイプ
  */
  typedef enum {
    TypeUnknown,                //!< 不明
    QT,                         //!< 停止コマンドを受信
    GD,                         //!< 逐次データ取得(距離 3 byte 表現)
    GS,                         //!< 逐次データ取得(距離 2 byte 表現)
    MD,                         //!< 自動データ取得(距離 3 byte 表現)
    MS,                         //!< 自動データ取得(距離 2 byte 表現)
    ME,                         //!< M コマンドの応答
    Mx_Reply,                   //!< Mx コマンドの最初の応答メッセージ
    InvalidData,                //!< 受信データのエラー
  } CaptureType;


  /*!
    \brief データ取得設定
  */
  class CaptureSettings
  {
  public:
    CaptureType type;           //!< 受信データの種類
    int error_code;             //!< エラーコード
    long timestamp;             //!< タイムスタンプ [msec]
    int capture_first;          //!< 取得開始インデックス
    int capture_last;           //!< 取得終了インデックス
    int skip_lines;             //!< 間引きライン数
    int skip_frames;            //!< データ取得間隔
    int remain_times;           //!< データ取得の残り回数
    int data_byte;              //!< データバイト数


    CaptureSettings(void)
      : type(TypeUnknown), error_code(-1), timestamp(-1),
        capture_first(-1), capture_last(-1),
        skip_lines(-1), skip_frames(-1), remain_times(-1), data_byte(-1)
    {
    }
  };


  void clearReceived(vector<long>& data, CaptureType& type,
                     int& line_count, int& timeout,
                     string& remain_string, string& left_packet_data,
                     int& store_times)
  {
    data.clear();
    type = TypeUnknown;
    line_count = 0;
    timeout = 0;
    remain_string.clear();
    left_packet_data.clear();
    store_times = 0;
  }
}


struct ScipHandler::pImpl
{
  enum {
    TotalTimeout = 1000,        // [msec]
    ContinuousTimeout = 100,    // [msec]
    FirstTimeout = 1000,        // [msec]

    BufferSize = 64 + 1 + 1 + 16, // データ長 + チェックサム + 改行 + 予備

    ResponseTimeout = -1,
    MismatchResponse = -2,
    SendFail = -3,
    ChecksumFail = -4,
    Scip11Response = -14,
  };


  typedef enum {
    Laser_Unknown = 0,
    Laser_On,
    Laser_Off,
  } LaserState;


  string error_message_;
  Connection* connection_;
  LaserState laser_state_;
  bool mx_capturing_;

  bool isPreCommand_QT_;


  pImpl(void)
    : error_message_("not connected."), connection_(NULL),
      laser_state_(Laser_Unknown), mx_capturing_(false),
      isPreCommand_QT_(false)
  {
  }


  bool connect(const char* device, long baudrate)
  {
    if (! connection_->connect(device, baudrate)) {
      error_message_ = connection_->what();
      return false;
    }

    long try_baudrates[] = { 115200, 19200, 38400, };
    size_t try_size = sizeof(try_baudrates) / sizeof(try_baudrates[0]);

    // 接続したいボーレートを配列の先頭要素と入れ換える
    for (size_t i = 1; i < try_size; ++i) {
      if (baudrate == try_baudrates[i]) {
        swap(try_baudrates[0], try_baudrates[i]);
        break;
      }
    }

    // 指定のボーレートで接続し、応答が返されるかどうか試す
    for (size_t i = 0; i < try_size; ++i) {

      // ホスト側のボーレートを変更
      if (! connection_->setBaudrate(try_baudrates[i])) {
        error_message_ = connection_->what();
        return false;
      }

      // 前回分の受信パケットを読み捨てる
      connection_->clear();

      // QT の発行
      int return_code = -1;
      char qt_expected_response[] = { 0, -1 };
      // return_code を使いたいため、setLaserOutput() を用いずに QT を送信する
      if (response(return_code, "QT\n", qt_expected_response)) {
        laser_state_ = Laser_Off;
        return changeBothBaudrate(baudrate);

      } else if (return_code == ResponseTimeout) {
        // ボーレートが違っていて、通信できなかったとみなす
        error_message_ = "baudrate is not detected.";
        continue;

      } else if (return_code == MismatchResponse) {
        // MD/MS の応答とみなし、受信データを読み飛ばす
	connection_->clear();
        skip(connection_, ContinuousTimeout);
        return changeBothBaudrate(baudrate);

      } else if (return_code == Scip11Response) {
        // SCIP1.1 プロトコルの場合のみ、SCIP2.0 を送信する
        char scip20_expected_response[] = { 0, -1 };
        if (! response(return_code, "SCIP2.0\n", scip20_expected_response)) {
          error_message_ =
            "SCIP1.1 protocol is not supported. Please update URG firmware, or reconnect after a few seconds because sensor is booting.";
          return false;
        }
        return changeBothBaudrate(baudrate);

      } else if (return_code == 0xE) {
        // TM モードとみなし、TM2 を発行する
        char tm2_expected_response[] = { 0, -1 };
        if (response(return_code, "TM2\n", tm2_expected_response)) {
          laser_state_ = Laser_Off;
          return changeBothBaudrate(baudrate);
        }
      }
    }

    return false;
  }


  bool response(int& return_code, const char send_command[],
                char expected_response[],
                vector<string>* lines = NULL)
  {
    return_code = -1;
    if (! connection_) {
      error_message_ = "no connection.";
      return false;
    }

    size_t send_size = strlen(send_command);
    int actual_send_size = connection_->send(send_command, send_size);
    if (strncmp(send_command, "QT\n", send_size)) {
      isPreCommand_QT_ = false;
    }
    if (actual_send_size != static_cast<int>(send_size)) {
      return_code = SendFail;
      return false;
    }

    // エコーバックの受信
    char buffer[BufferSize];
    int recv_size = readline(connection_, buffer, BufferSize, FirstTimeout);
    if (recv_size < 0) {
      error_message_ = "response timeout: "
        + string(send_command).substr(0, strlen(send_command) - 1);
      return_code = ResponseTimeout;
      return false;
    }

    // シリアル接続でボーレート変更直後の 0x00 は、判定外とする
    if (! ((recv_size == 1) && (buffer[0] == 0x00))) {
      if ((recv_size != static_cast<int>(send_size - 1)) ||
          (strncmp(buffer, send_command, recv_size))) {
        error_message_ = "mismatch response: " + string(buffer);
        return_code = MismatchResponse;
        return false;
      }
    }

    // 応答の受信
    // SCIP1.1 プロトコルの応答は、負号を付加した上で return_code に格納する
    recv_size = readline(connection_, buffer, BufferSize, ContinuousTimeout);
    if (recv_size < 0) {
      error_message_ = "response timeout.";
      return_code = ResponseTimeout;
      return false;
    }
    if (recv_size == 3) {
      // ３文字ならば、SCIP2.0 とみなしてチェックサムを確認する
      if (! checkSum(buffer, recv_size - 1, buffer[recv_size - 1])) {
	return_code = ChecksumFail;
	return false;
      }
      buffer[2] = '\0';
      return_code = strtol(buffer, NULL, 16);

    } else if (recv_size == 1) {
      // １文字ならば、SCIP1.1 とみなして 16進変換した値に負号をつけて返す
      buffer[1] = '\0';
      return_code = -strtol(buffer, NULL, 16);
    }

    // データ領域の受信
    // １行読み出し、改行のみならば終了とみなす
    do {
      recv_size = readline(connection_, buffer, BufferSize, ContinuousTimeout);
      if (lines && (recv_size > 0)) {
        lines->push_back(buffer);
      }
    } while (recv_size > 0);

    for (int i = 0; expected_response[i] != -1; ++i) {
      if (return_code == expected_response[i]) {
        return true;
      }
    }
    return false;
  }


  bool changeBothBaudrate(long baudrate)
  {
    // 既に目標対象のボーレート値ならば、成功とみなす
    // この関数は、ScipHandler::connect() か、それ以後でないと呼ばれないため
    if (connection_->baudrate() == baudrate) {
      return true;
    }

    // URG 側のボーレートを変更
    int pre_ticks = ticks();
    if (! changeBaudrate(baudrate)) {
      return false;
    }

    // シリアル通信の場合、ボーレート変更後、１周分だけ待つ必要がある
    int reply_msec = ticks() - pre_ticks;
    delay((reply_msec * 4 / 3) + 10);

    // ホスト側のボーレートを変更
    return connection_->setBaudrate(baudrate);
  }


  bool changeBaudrate(long baudrate)
  {
    if (! ((baudrate == 19200) || (baudrate == 38400) ||
           (baudrate == 57600) || (baudrate == 115200))) {
      baudrate = 115200;
      //error_message_ = "Invalid baudrate value.";
      //return false;
    }

    // SS を送信し、URG 側のボーレートを変更する
    char send_buffer[] = "SSxxxxxx\n";
    snprintf(send_buffer, 10, "SS%06ld\n", baudrate);
    int return_code = -1;
    // !!! 既に設定対象のボーレート、の場合の戻り値を ss_expected... に追加する
    char ss_expected_response[] = { 0, 3, 4, -1 };
    if (! response(return_code, send_buffer, ss_expected_response)) {
      error_message_ = "Baudrate change fail.";
      return false;
    }

    return true;
  }


  bool loadParameter(RangeFinderParameter& parameter)
  {
    // PP の送信とデータの受信
    int return_code = -1;
    char pp_expected_response[] = { 0, -1 };
    vector<string> lines;
    if (! response(return_code, "PP\n", pp_expected_response, &lines)) {
      error_message_ = "PP fail.";
      return false;
    }

    // PP 応答内容の格納
    enum { PP_Lines = 8 };
    if (lines.size() != PP_Lines) {
      error_message_ = "Invalid PP response.";
      return false;
    }

    for (size_t i = 0; i < PP_Lines; ++i) {
      string& line = lines[i];
      size_t n = line.size();

      // PP は末尾の ';' はチェックサムの文字に含まない
      if ((! checkSum(&line[0], n - 2, line[n - 1], false)) &&
          (! checkSum(&line[0], n - 1, line[n - 1]))) {
        error_message_ = "PP checksum error.";
        return false;
      }
    }

    int modl_length =
      static_cast<int>(lines[RangeFinderParameter::MODL].size());
    // 最初のタグと、チェックサムを除いた文字列を返す
    if (modl_length > (5 + 2)) {
      modl_length -= (5 + 2);
    }
    parameter.model = lines[RangeFinderParameter::MODL].substr(5, modl_length);

    parameter.distance_min = substr2int(lines[RangeFinderParameter::DMIN], 5);
    parameter.distance_max = substr2int(lines[RangeFinderParameter::DMAX], 5);
    parameter.area_total = substr2int(lines[RangeFinderParameter::ARES], 5);
    parameter.area_min = substr2int(lines[RangeFinderParameter::AMIN], 5);
    parameter.area_max = substr2int(lines[RangeFinderParameter::AMAX], 5);
    parameter.area_front = substr2int(lines[RangeFinderParameter::AFRT], 5);
    parameter.scan_rpm = substr2int(lines[RangeFinderParameter::SCAN], 5);

    return true;
  }


  int substr2int(const string& line, int from_n, int length = string::npos)
  {
    return atoi(line.substr(from_n, length).c_str());
  }


  bool receiveData(std::vector<long>& data, long* timestamp)
  {
    string remain_string;
    string left_packet_data;
    char buffer[BufferSize];

    int store_times = 0;
    int line_count = 0;
    data.clear();

    CaptureSettings settings;

    error_message_ = "no response.";

    // !!! 以下の処理が長い。関数化すべき
    CaptureType type = TypeUnknown;
    int timeout = FirstTimeout;
    int line_size = 0;
    while ((line_size = readline(connection_,
                                 buffer, BufferSize, timeout)) > 0) {
      //fprintf(stderr, "% 3d: %s\n", line_count, buffer);

      // チェックサムの確認
      if (line_count != 0) {
        // エコーバックにはチェックサム文字列がないので、無視
        if (! checkSum(buffer, line_size - 1, buffer[line_size - 1])) {
          log_printf("checksum error: %s\n", buffer);

          // 存在するだけのパケットを読み飛ばす
          error_message_ = "invalid packet.";
          clearReceived(data, type, line_count, timeout,
                        remain_string, left_packet_data, store_times);
          continue;
        }
      }

      if (line_count == 0) {

        // エコーバック
        string line = buffer;
        if ((! line.compare(0, 2, "GD")) || (! line.compare(0, 2, "GS"))) {
          if (! parseGdEchoback(settings, line)) {
            break;
          }
          type = (line[1] = 'D') ? GD : GS;
          data.reserve(settings.capture_last + 1);

        } else if ((! line.compare(0, 2, "MD")) ||
                   (! line.compare(0, 2, "MS"))) {
          if (! parseMdEchoback(settings, line)) {
            break;
          }
          type = (line[1] = 'D') ? MD : MS;
          laser_state_ = Laser_On;
          data.reserve(settings.capture_last + 1);

        } else if (! line.compare(0, 2, "ME")) {
          if (! parseMeEchoback(settings, line)) {
            break;
          }
          type = ME;
          laser_state_ = Laser_On;
          data.reserve(settings.capture_last + 1);

        } else if (! line.compare(0, 2, "QT")) {
          settings.remain_times = 0;
          laser_state_ = Laser_Off;
          mx_capturing_ = false;

        } else {
          //fprintf(stderr, "clear 1\n");
          clearReceived(data, type, line_count, timeout,
                        remain_string, left_packet_data, store_times);
          continue;
        }

      } else if (line_count == 1) {
        // 応答コード
        if (line_size != 3) {
          //fprintf(stderr, "clear 2\n");
          clearReceived(data, type, line_count, timeout,
                        remain_string, left_packet_data, store_times);
          continue;
        }
        buffer[2] = '\0';
        settings.error_code = atoi(buffer);

        // "0B" が返された場合、センサとホストの応答がずれている可能性があるので
        // 続く応答を読み捨てる
        if (! strncmp(buffer, "0B", 2)) {
          skip(connection_, TotalTimeout, timeout);
        }

        if ((settings.error_code == 0) &&
            ((type == MD) || (type == MS) || (type == ME))) {
          type = Mx_Reply;
        }

      } else if (line_count == 2) {
        // タイムスタンプ
        if (timestamp) {
          *timestamp = encode(buffer, 4);
        }
      } else {
        if (line_count == 3) {
          // 受信データがない先頭からの領域を、ダミーデータで埋める
          for (int i = 0; i < settings.capture_first; ++i) {
            data.push_back(0);
            //++store_times;
            if (type == ME) {
              // ME 受信のときは、強度データ分も埋める
              data.push_back(0);
              //++store_times;
            }
          }
          //fprintf(stderr, "store_times: %d\n", store_times);
        }
        // 距離データ
        left_packet_data =
          addLengthData(data, string(buffer),
                        left_packet_data, settings.data_byte,
                        store_times, settings.skip_lines);
      }
      ++line_count;
      timeout = ContinuousTimeout;
    }

    // ME で "まとめる数" 設定以上のデータが返されるバグに対処
    size_t expected_n = settings.capture_last * ((type == ME) ? 2 : 1) + 1;
    if (expected_n < data.size()) {
      //fprintf(stderr, "data.size: %d, %d\n", data.size(), expected_n);
      data.erase(data.begin() + expected_n, data.end());
      if (store_times > static_cast<int>(expected_n)) {
        store_times = expected_n - 1;
      }
      //fprintf(stderr, "expected_n: %d\n", expected_n);
    }

    // type が距離データ取得のときは、正常に受信が完了したか、を確認する
    int expected_store_times =
      settings.capture_last - settings.capture_first + 1;
    if (store_times != expected_store_times) {
      //fprintf(stderr, "expected_n: %d\n", expected_n);
      //fprintf(stderr, "store_times: %d, %d\n", store_times, expected_store_times);
      error_message_ = "capture fail.";
      return false;
    }

    return true;
  }


  bool parseGdEchoback(CaptureSettings& settings, const string& line)
  {
    if (line.size() != 12) {
      error_message_ = "Invalid Gx packet has arrived.";
      return false;
    }

    settings.capture_first = substr2int(line, 2, 4);
    settings.capture_last = substr2int(line, 6, 4);
    int skip_lines = substr2int(line, 10, 2);
    settings.skip_lines = (skip_lines == 0) ? 1 : skip_lines;
    settings.data_byte = (line[1] == 'D') ? 3 : 2;

    return true;
  }


  bool parseMdEchoback(CaptureSettings& settings, const string& line)
  {
    if (line.size() != 15) {
      error_message_ = "Invalid Mx packet has arrived.";
      return false;
    }

    settings.capture_first = substr2int(line, 2, 4);
    settings.capture_last = substr2int(line, 6, 4);
    int skip_lines = substr2int(line, 10, 2);
    settings.skip_lines = (skip_lines == 0) ? 1 : skip_lines;
    settings.skip_frames = substr2int(line, 12, 1);
    settings.remain_times = substr2int(line, 13, 2);
    settings.data_byte = (line[1] == 'D') ? 3 : 2;

    if (settings.remain_times == 1) {
      // 最後のデータ取得で、レーザを消灯扱いにする
      // 本当は、次のデータ取得後にレーザは消灯されている
      // 1 で判定すると、取得回数が 1 のときにも正常に動作する
      mx_capturing_ = false;

    } else if (settings.remain_times > 0) {
      mx_capturing_ = true;
    }

    return true;
  }


  bool parseMeEchoback(CaptureSettings& settings, const string& line)
  {
    if (line.size() != 15) {
      error_message_ = "Invalid ME packet has arrived.";
      return false;
    }

    settings.capture_first = substr2int(line, 2, 4);
    settings.capture_last = substr2int(line, 6, 4);
    int skip_lines = substr2int(line, 10, 2);
    settings.skip_lines = (skip_lines == 0) ? 1 : skip_lines;
    settings.skip_frames = substr2int(line, 12, 1);
    settings.remain_times = substr2int(line, 13, 2);
    settings.data_byte = 3;

    if (settings.remain_times == 1) {
      mx_capturing_ = false;

    } else {
      mx_capturing_ = true;
    }

    return true;
  }


  string addLengthData(vector<long>& data,
                       const string& line,
                       const string& left_packet_data,
                       const size_t data_byte, int& store_times,
                       const int skip_lines = 1)
  {
    if (line.empty()) {
      // 空行の場合、戻る
      return left_packet_data;
    }

    // 端数。次回に処理する分
    string left_byte = left_packet_data;

    size_t data_size = (left_byte.size() + (line.size() - 1)) / data_byte;
    size_t n = data_size * data_byte - left_byte.size();
    for (size_t i = 0; i < n; ++i) {
      left_byte.push_back(line[i]);
      if (left_byte.size() >= data_byte) {
        // データを距離に変換して、格納
        long length = encode(&left_byte[0], data_byte);
        for (int j = 0; j < skip_lines; ++j) {
          data.push_back(length);
          ++store_times;
        }
        left_byte.clear();
      }
    }
    left_byte += line.substr(n, (line.size() - n) - 1);

    return left_byte;
  }


  int send(const char data[], int size)
  {
    if (size >= 2) {
      // 送信コマンドがレーザを発光させるかを判定する
      if ((! strncmp("MD", data, 2)) || (! strncmp("ME", data, 2)) ||
          (! strncmp("MS", data, 2))) {
        laser_state_ = Laser_On;
        mx_capturing_ = true;
        isPreCommand_QT_ = false;
      }
    }
    return connection_->send(data, size);
  }


  bool setLaserOutput(bool on, bool force)
  {
    // !!! force を使わないならば、削除する

    if (((on == true) && (laser_state_ == Laser_On)) ||
        ((on == false) && (laser_state_ == Laser_Off))) {
      if (! force) {
        // レーザ出力が現在の状態と同じならば戻る
        // 強制設定フラグが true のときは戻らずに設定を行う
        return true;
      }
    }
    if ((!on) && isPreCommand_QT_) {
      return false;
    }

    if (on) {
      int return_code = -1;
      char expected_response[] = { 0, -1 };
      if (! response(return_code, "BM\n", expected_response)) {
        error_message_ = "BM fail.";
        return false;
      }
      laser_state_ = Laser_On;
      return true;

    } else {
      // "QT"
      if (! mx_capturing_) {
        // 消灯するための QT では、応答を待つべき
        int return_code = -1;
        char qt_expected_response[] = { 0, -1 };
        if (! response(return_code, "QT\n", qt_expected_response)) {
          return false;
        }
        laser_state_ = Laser_Off;
        isPreCommand_QT_ = true;
        return true;

      } else {
        // MD を中断するための QT では、応答を待ってはならない
        // 応答は、受信スレッド内で処理される
        connection_->send("QT\n", 3);
        isPreCommand_QT_ = true;
      }

      return true;
    }
  }
};


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


ScipHandler::~ScipHandler(void)
{
}


bool ScipHandler::checkSum(char* buffer, int size, char actual_sum,
                           bool print_error)
{
  char expected_sum = 0x00;
  for (int i = 0; i < size; ++i) {
    expected_sum += buffer[i];
  }
  expected_sum = (expected_sum & 0x3f) + 0x30;

  if (print_error && (expected_sum != actual_sum)) {
    log_printf("checksum error!\n");
    for (int i = 0; i < size; ++i) {
      log_printf("%c", buffer[i]);
    }
    log_printf(", %d\n", size);
  }

  return (expected_sum == actual_sum) ? true : false;
}


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


long ScipHandler::timestamp(void) const
{
  int return_code = -1;
  char expected_response[] = { 0, -1 };
  if (! pimpl->response(return_code, "TM0\n", expected_response)) {
    return -1;
  }

  long timestamp = -1;
  vector<string> lines;
  long first_ticks = ticks();
  long last_ticks = first_ticks;
  if (pimpl->response(return_code, "TM1\n", expected_response, &lines)) {
    last_ticks = ticks();
    if (! lines.empty()) {
      timestamp = encode(lines[0].c_str(), 4);
    }
  }

  if (! pimpl->response(return_code, "TM2\n", expected_response)) {
    return -1;
  }

  long end_ticks = ticks();
  timestamp += ((last_ticks - first_ticks) / 2) + (end_ticks - last_ticks);

  return timestamp;
}


void ScipHandler::setConnection(Connection* connection)
{
  pimpl->connection_ = connection;
}


Connection* ScipHandler::connection(void) const
{
  return pimpl->connection_;
}


bool ScipHandler::connect(const char* device, long baudrate)
{
  return pimpl->connect(device, baudrate);
}


bool ScipHandler::versionLines(std::vector<std::string>& lines)
{
  int return_code = -1;
  char expected_response[] = { 0, -1 };
  if (! pimpl->response(return_code, "VV\n", expected_response, &lines)) {
    return false;
  }
  return true;
}


bool ScipHandler::loadParameter(RangeFinderParameter& parameter)
{
  return pimpl->loadParameter(parameter);
}


int ScipHandler::send(const char data[], int size)
{
  return pimpl->send(data, size);
}


int ScipHandler::receive(char data[], int max_size, int timeout)
{
  return pimpl->connection_->receive(data, max_size, timeout);
}


bool ScipHandler::receiveData(std::vector<long>& data, long* timestamp)
{
  return pimpl->receiveData(data, timestamp);
}


void ScipHandler::laserOn(void)
{
  pimpl->setLaserOutput(true, false);
}


void ScipHandler::laserOff(void)
{
  pimpl->setLaserOutput(false, false);
}


long ScipHandler::encode(const char* data, size_t size)
{
  int value = 0;

  for (size_t i = 0; i < size; ++i) {
    value <<= 6;
    value &= ~0x3f;
    value |= data[i] - 0x30;
  }
  return value;
}
