/*!
  \file
  \brief UrgDevice のシミュレータ

  \author Satofumi KAMIMURA

  $Id: UrgDevice_device.cpp 1678 2010-02-08 23:46:31Z satofumi $

  \todo setSkipLines(2) を指定すると、データの記録開始位置がずれるのを修正する
*/

#include "UrgDevice_device.h"
#include "UrgModel.h"
#include "RangeFinderParameter.h"
#include "DeviceManager.h"
#include "TcpipSocket.h"
#include "TcpipAccepter.h"
#include "ConnectionUtils.h"
#include "log_printf.h"
#include <cstring>
#include <vector>

using namespace qrk;
using namespace std;


struct UrgDevice_device::pImpl
{
  typedef enum {
    Measuring,                  //!< 計測中
    Calculating,                //!< 距離計算中
    Waiting,                    //!< 計測の開始待ち中
  } UrgState;


  typedef enum {
    On,
    Off,
  } LaserState;


  typedef enum {
    None,
    Response_GD,
    Response_MD,
  } DelayedResponse;


  enum {
    BufferSize = 128,
  };


  TcpipAccepter tcpip_accepter_;
  long timestamp_;
  UrgModel urg_model_;
  RangeFinderParameter parameter_;

  Connection* connection_;

  string type_;
  UrgState state_;
  size_t scan_msec_;
  size_t scan_begin_msec_;
  size_t scan_end_msec_;

  size_t scan_timestamp_;

  string command_;
  char receive_buffer_[BufferSize];
  size_t filled_;
  char lf_ch_;
  int ss_return_code_;
  long max_length_;
  LaserState laser_state_;

  DelayedResponse delayed_response_;
  size_t capture_first_;
  size_t capture_last_;
  size_t capture_cluster_;
  size_t previous_captured_;
  vector<long> distance_data_;


  pImpl(const char* device)
    : timestamp_(0), connection_(NULL), state_(Waiting),
      scan_msec_(1), scan_begin_msec_(0), scan_end_msec_(1),
      scan_timestamp_(0), filled_(0), lf_ch_('\n'),
      ss_return_code_(0), max_length_(0),
      laser_state_(Off), delayed_response_(None),
      capture_first_(0), capture_last_(0), capture_cluster_(1),
      previous_captured_(0)
  {
    DeviceManager device_manager;
    for (int i = 0; i < PortRetryMax; ++i) {
      long try_port = device_manager.nextPort();
      if (tcpip_accepter_.activate(try_port)) {
        // ポートの登録と URG デバイスとしての初期化
        device_manager.registerDevicePort(device, try_port);
        return;
      }
    }

    log_printf("UrgDevice_device(): fail assign port.\n");
    throw;
  }


  void urg_initialize(const char* type)
  {
    // パラメータを初期化し、タイムスタンプをクリアする
    timestamp_ = 0;

    if (! strcmp(type, "URG-04LX")) {
      parameter_.model = "MODL:URG-04LX(Hokuyo Automatic Co. Ltd.)";
      parameter_.distance_min = 20;
      parameter_.distance_max = 5600;
      parameter_.area_total = 1024;
      parameter_.area_min = 44;
      parameter_.area_max = 725;
      parameter_.area_front = 384;
      parameter_.scan_rpm = 600;

      max_length_ = 5600;
      ss_return_code_ = 0;

    } else {
      // "UTM-30LX"
      parameter_.model = "MODL:UTM-30LX(Hokuyo Automatic Co.,Ltd.)";
      parameter_.distance_min = 23;
      parameter_.distance_max = 60000;
      parameter_.area_total = 1440;
      parameter_.area_min = 0;
      parameter_.area_max = 1080;
      parameter_.area_front = 540;
      parameter_.scan_rpm = 2400;

      max_length_ = 30000;
      ss_return_code_ = 4;
    }

    scan_msec_ = 1000 * 60 / parameter_.scan_rpm;
    size_t waiting_msec =
      (parameter_.area_total - (parameter_.area_max - parameter_.area_min)) *
      scan_msec_ / parameter_.area_total;
    scan_begin_msec_ = waiting_msec / 2;
    scan_end_msec_ = scan_msec_ - (waiting_msec / 2);
  }


  bool accept(void)
  {
    connection_ = tcpip_accepter_.accept(0);
    return (connection_) ? true : false;
  }


  void disconnect(void)
  {
    if (connection_) {
      connection_->disconnect();
      connection_ = NULL;
    }
  }


  void senseAround(void)
  {
    ++timestamp_;
    size_t timing = timestamp_ % scan_msec_;

    // 取得向きを変更しつつ、position() 位置からの対象物との距離を計算する
    switch (state_) {
    case Measuring:
      measureDistance(timing);
      if (timing > scan_end_msec_) {
        setLasers(previous_captured_, capture_last_ + 1, capture_cluster_);
        state_ = Calculating;
      }
      break;

    case Calculating:
      // !!! 必要ならば、通信を返せるようになるまでの遅延を適用する
      // !!! 遅延時間はセンサ依存
      state_ = Waiting;
      delayedResponse();
      handlePacket();
      break;

    case Waiting:
      if (timing > scan_begin_msec_) {
        state_ = Measuring;
        distance_data_.clear();
        distance_data_.reserve(capture_last_ - capture_first_ + 1);
        scan_timestamp_ = timestamp_;
      }
      break;
    }
  }


  // 前回コマンドの応答を返す処理
  void delayedResponse(void)
  {
    switch (delayed_response_) {
    case None:
      break;

    case Response_GD:
      responseGD();
      delayed_response_ = None;
      break;

    case Response_MD:
      // !!! MD 応答の処理
      break;
    }
  }


  void measureDistance(size_t timing)
  {
    if (laser_state_ != On) {
      return;
    }

    // !!! URG の位置取得は urg_model_ を用いて行う

    // !!! 距離データを取得する範囲を計算する

    size_t current_index =
      parameter_.area_total * (timing - scan_begin_msec_) / scan_msec_;

    // レーザの配置
    setLasers(previous_captured_, current_index, capture_cluster_);
    previous_captured_ = max(current_index, previous_captured_);
  }


  void setLasers(size_t first_index, size_t last_index, size_t skip_lines)
  {
    if (! ((delayed_response_ == Response_GD) ||
           (delayed_response_ == Response_MD))) {
      return;
    }

    last_index = min(last_index, capture_last_ + 1);
    if (first_index > last_index) {
      return;
    }

    for (size_t i = first_index; i < last_index; i += skip_lines) {
      // 距離のデフォルト値を指定し、レーザーをシミュレーション環境上に配置する
      distance_data_.push_back(1);

      int index_from_front = i - parameter_.area_front;
      double radian = index_from_front * (2.0 * M_PI) / parameter_.area_total;
      urg_model_.setLaser(&distance_data_.back(), radian, max_length_);
    }
  }


  void handlePacket(void)
  {
    int n = connection_->receive(&receive_buffer_[filled_],
                                 BufferSize - 1 - filled_, 0);
    if (n <= 0) {
      if (n < 0) {
        // !!! エラー表示
        disconnect();
      }
      return;
    }

    filled_ += n;

    receive_buffer_[filled_] = '\0';
    char* last_ch = strchr(receive_buffer_, '\r');
    if (! last_ch) {
      last_ch = strchr(receive_buffer_, '\n');
    }

    if (last_ch) {
      // パケットの切りだしと処理
      lf_ch_ = *last_ch;
      *last_ch = '\0';
      string packet = receive_buffer_;

      char* next_first_ch = ++last_ch;
      if (isLF(*next_first_ch)) {
        ++next_first_ch;
      }

      size_t move_size = next_first_ch - receive_buffer_;
      memmove(receive_buffer_, next_first_ch, move_size);
      filled_ -= move_size;

      // SCIP パケットの処理
      handleScip(packet);
    }
  }


  void handleScip(const string& packet)
  {
    //fprintf(stderr, "packet: %s\n", packet.c_str());

    if (! packet.compare("QT")) {
      // MD 取得の停止処理
      // !!!
      response(packet, 0);

    } else if (delayed_response_ != None) {
      // MD 中はここまでのコマンド以外を受け付けない
      // !!! エラー応答を返す
      fprintf(stderr, "delayed_response_ != None\n");
      return;

    } else if (! packet.compare(0, 2, "GD")) {
      receiveGD(packet);

    } else if (! packet.compare(0, 2, "SS")) {
      responseSS(packet);

    } else if (! packet.compare("PP")) {
      responsePP();

    } else if (! packet.compare("BM")) {
      int ret = 0;
      if (laser_state_ == On) {
        ret = 2;
      }
      laser_state_ = On;
      response(packet, ret);
    }
  }


  char checksum(const char* buffer, size_t n)
  {
    (void)buffer;
    (void)n;

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


  void response(const string& packet, int return_code)
  {
    char buffer[BufferSize];

    snprintf(buffer, BufferSize, "%s%c", packet.c_str(), lf_ch_);
    connection_->send(buffer, strlen(buffer));
    snprintf(buffer, BufferSize, "%02dx%c", return_code, lf_ch_);
    buffer[2] = checksum(buffer, 2);
    connection_->send(buffer, strlen(buffer));
  }


  void receiveGD(const string& packet)
  {
    // レーザが消灯中の場合に、エラー応答を返す
    // !!!

    command_ = packet;
    capture_first_ = atoi(packet.substr(2, 4).c_str());
    capture_last_ = atoi(packet.substr(6, 4).c_str());
    capture_cluster_ = atoi(packet.substr(10, 2).c_str());
    previous_captured_ = capture_first_;

    // 計測と応答を指示する
    delayed_response_ = Response_GD;
  }


  void responseGD(void)
  {
    //fprintf(stderr, "data_size: %d\n", distance_data_.size());
    // !!!

    // エコーバック
    response(command_.c_str(), 0);

    // タイムスタンプ
    char buffer[BufferSize];
    encode(buffer, scan_timestamp_, 4);
    buffer[4] = checksum(buffer, 4);
    buffer[5] = lf_ch_;
    connection_->send(buffer, 6);

    // 距離データ
    enum {
      DataSize = 3,
      LineLength = 64,
    };
    size_t n = distance_data_.size();
    size_t filled = 0;
    char checksum_buffer[3];
    checksum_buffer[1] = lf_ch_;
    checksum_buffer[2] = lf_ch_;
    for (size_t i = 0; i < n; ++i) {
      // !!! GS, MS には未対応
      encode(&buffer[filled], distance_data_[i], DataSize);
      filled += DataSize;

      if (filled > LineLength) {
        checksum_buffer[0] = checksum(buffer, LineLength);
        connection_->send(buffer, LineLength);
        connection_->send(checksum_buffer, 2);

        size_t move_size = filled - LineLength;
        memmove(buffer, &buffer[filled - move_size], move_size);
        filled = move_size;
      }
    }
    checksum_buffer[0] = checksum(buffer, filled);
    connection_->send(buffer, filled);
    if (filled > 0) {
      connection_->send(checksum_buffer, 3);
    }
  }


  void encode(char* encoded, size_t value, size_t byte_size)
  {
    for (size_t i = 0; i < byte_size; ++i) {
      encoded[byte_size - 1 - i] = (value & 0x3f) + 0x30;
      value >>= 6;
    }
  }


  void responseSS(const string& packet)
  {
    // 想定されるボーレートの場合に正常応答を返し、それ以外はエラー応答を返す
    long baudrate = strtol(&packet.c_str()[2], NULL, 10);
    if (! ((baudrate == 19200) || (baudrate == 38400) ||
           (baudrate == 57600) || (baudrate == 115200))) {
      // !!! パラメータエラー
      fprintf(stderr, "responseSS: %ld\n", baudrate);
    }

    response(packet, ss_return_code_);
  }


  void responsePP(void)
  {
    response("PP", 0);

    // パラメータ情報を返す
    enum { PP_BufferSize = BufferSize * 8 };
    char buffer[PP_BufferSize];
    size_t filled = 0;

    int n = snprintf(buffer, BufferSize,
                     "%s;x%c", parameter_.model.c_str(), lf_ch_);
    buffer[n - 2] = checksum(buffer,  n - 2);
    filled += n;

    // ';' がチェックサムに含まれないバグもシミュレートする
    n = snprintf(&buffer[filled], BufferSize,
                 "DMIN:%ld;x%c", parameter_.distance_min, lf_ch_);
    buffer[filled + n - 2] = checksum(&buffer[filled], n - 3);
    filled += n;

    n = snprintf(&buffer[filled], BufferSize,
                 "DMAX:%ld;x%c", parameter_.distance_max, lf_ch_);
    buffer[filled + n - 2] = checksum(&buffer[filled], n - 3);
    filled += n;

    n = snprintf(&buffer[filled], BufferSize,
                 "ARES:%d;x%c", parameter_.area_total, lf_ch_);
    buffer[filled + n - 2] = checksum(&buffer[filled], n - 3);
    filled += n;

    n = snprintf(&buffer[filled], BufferSize,
                 "AMIN:%d;x%c", parameter_.area_min, lf_ch_);
    buffer[filled + n - 2] = checksum(&buffer[filled], n - 3);
    filled += n;

    n = snprintf(&buffer[filled], BufferSize,
                 "AMAX:%d;x%c", parameter_.area_max, lf_ch_);
    buffer[filled + n - 2] = checksum(&buffer[filled], n - 3);
    filled += n;

    n = snprintf(&buffer[filled], BufferSize,
                 "AFRT:%d;x%c", parameter_.area_front, lf_ch_);
    buffer[filled + n - 2] = checksum(&buffer[filled], n - 3);
    filled += n;

    n = snprintf(&buffer[filled], BufferSize,
                 "SCAN:%d;x%c", parameter_.scan_rpm, lf_ch_);
    buffer[filled + n - 2] = checksum(&buffer[filled], n - 3);
    filled += n;

    buffer[filled] = lf_ch_;
    ++filled;

    connection_->send(buffer, filled);
  }
};


UrgDevice_device::UrgDevice_device(const char* device)
  : pimpl(new pImpl(device))
{
}


UrgDevice_device::~UrgDevice_device(void)
{
}


void UrgDevice_device::setParameter(const char* type, const char* parameter)
{
  if (! strcmp(type, "Type")) {
    pimpl->type_ = parameter;
  }

  // !!! シリアル ID を変更できるようにする
}


void UrgDevice_device::activate(void)
{
  pimpl->urg_initialize(pimpl->type_.c_str());
  pimpl->urg_model_.activate();
}


void UrgDevice_device::execute(void)
{
  // 接続の受け付け
  if (! pimpl->connection_) {
    if (! pimpl->accept()) {
      // 接続なし
      return;
    }
  }

  //fprintf(stderr, ".");

  // 計測処理。通信処理は１周の計測において１回のみ評価する
  pimpl->senseAround();
}


size_t UrgDevice_device::nextExecuteInterval(void) const
{
  // !!!通信処理後、データ取得開始までの時間を指定してもよい
  return 1;
}


void UrgDevice_device::setPosition(const qrk::Position<long>& position,
                                   OdeModel* model, bool fixed)
{
  pimpl->urg_model_.setPosition(position, model, fixed);
}


Position<long> UrgDevice_device::position(void)
{
  return pimpl->urg_model_.position();
}


void UrgDevice_device::setPosition3d(const Point3d<long>& position,
                                     double ax, double ay, double az,
                                     const Angle& angle,
                                     OdeModel* model, bool fixed)
{
  pimpl->urg_model_.setPosition3d(position, ax, ay, az, angle, model, fixed);
}


void UrgDevice_device::position3d(long& px, long& py, long& pz,
                                  double& dx, double& dy, double& dz)
{
  (void)px;
  (void)py;
  (void)pz;
  (void)dx;
  (void)dy;
  (void)dz;

  // !!!
}


dBodyID UrgDevice_device::objectId(void) const
{
  return pimpl->urg_model_.objectId();
}
