/*!
  \file
  \brief URG の制御

  \author Satofumi KAMIMURA

  $Id: UrgDevice.cpp 1669 2010-02-04 08:28:27Z satofumi $
*/

#include "UrgDevice.h"
#include "ScipHandler.h"
#include "RangeFinderParameter.h"
#include "SerialDevice.h"
#include <cstring>
#include <cstdio>

using namespace qrk;
using namespace std;


struct UrgDevice::pImpl
{
  string error_message_;
  Connection* connection_;
  bool serial_created_;
  ScipHandler scip_;
  RangeFinderParameter parameters_;
  string urg_type_;

  CaptureMode capture_mode_;
  int capture_begin_;
  int capture_end_;
  int capture_skip_lines_;
  int capture_times_;
  int capture_frame_interval_;

  long base_timestamp_;
  long pre_timestamp_;


  pImpl(void)
    : error_message_("not connected."),
      connection_(NULL), serial_created_(false),
      capture_mode_(GD_Capture), capture_begin_(0), capture_end_(0),
      capture_skip_lines_(1), capture_times_(0), capture_frame_interval_(0),
      base_timestamp_(0), pre_timestamp_(0)
  {
  }


  ~pImpl(void)
  {
    if (serial_created_) {
      delete connection_;
    }
  }


  bool connect(const char* device, long baudrate)
  {
    disconnect();
    if (! connection_) {
      setSerialDevice();
    }
    scip_.setConnection(connection_);

    if (! scip_.connect(device, baudrate)) {
      disconnect();
      error_message_ = scip_.what();
      return false;
    }

    if (! loadParameter()) {
      disconnect();
      return false;
    }
    updateCaptureParameters();

    error_message_ = "connected.";
    return true;
  }


  void disconnect(void)
  {
    // !!! 接続前と同じ状態になるようにすべき

    stop();
    if (connection_) {
      connection_->disconnect();
    }
  }


  bool isConnected(void) const
  {
    return (connection_ == NULL) ? false : connection_->isConnected();
  }


  void setSerialDevice(void)
  {
    // シリアル接続のオブジェクトを生成する
    connection_ = new SerialDevice;
    serial_created_ = true;
  }


  void stop(void)
  {
    if (! isConnected()) {
      return;
    }

    // データ受信中ならば、QT を送信する
    scip_.laserOff();
  }


  bool loadParameter(void)
  {
    // URG パラメータの取得
    RangeFinderParameter parameters;
    if (! scip_.loadParameter(parameters)) {
      error_message_ = scip_.what();
      return false;
    }
    swap(parameters_, parameters);

    size_t type_length = parameters_.model.find('(');
    urg_type_ = parameters_.model.substr(0, type_length);

    return true;
  }


  void updateCaptureParameters(void)
  {
    capture_begin_ = parameters_.area_min;
    capture_end_ = parameters_.area_max;
  }


  void requestData(void)
  {
    char buffer[] = "\0Dbbbbeeeeggstt\n";
    size_t buffer_size = sizeof(buffer);

    switch (capture_mode_) {

    case GD_Capture:
      // レーザの点灯
      scip_.laserOn();

      snprintf(buffer, buffer_size, "GD%04d%04d%02u\n",
               capture_begin_, capture_end_,
               capture_skip_lines_);
      break;

    case MD_Capture:
      snprintf(buffer, buffer_size, "MD%04d%04d%02u%01u%02u\n",
               capture_begin_, capture_end_,
               capture_skip_lines_,
               capture_frame_interval_,
               (capture_times_ > 99) ? 0 : capture_times_);
      break;
    }

    scip_.send(buffer, strlen(buffer));
  }


  size_t rad2index(double radian) const
  {
    int area_total = parameters_.area_total;
    int index =
      static_cast<int>(floor(((radian * area_total) / (2.0 * M_PI)) + 0.5)
                       + parameters_.area_front);

    if (index < 0) {
      index = 0;
    } else if (index > parameters_.area_max) {
      index = parameters_.area_max;
    }
    return index;
  }
};


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


UrgDevice::~UrgDevice(void)
{
}


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


long UrgDevice::timestamp(void) const
{
  if (! isConnected()) {
    // !!! マクロにする
    return -1;
  }

  return pimpl->scip_.timestamp();
}


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


void UrgDevice::disconnect(void)
{
  pimpl->disconnect();
}


bool UrgDevice::isConnected(void) const
{
  return pimpl->isConnected();
}


void UrgDevice::setConnection(Connection* connection)
{
  pimpl->connection_ = connection;
  pimpl->scip_.setConnection(connection);

  // SerialDevice のリソース管理はユーザ側に任せる
  pimpl->serial_created_ = false;
}


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


bool UrgDevice::reboot(void)
{
  laserOff();

  // send "RB" twice.
  for (int i = 0; i < 2; ++i) {
    pimpl->scip_.send("RB\n", 3);

    enum {
      RB_Timeout = 1000,        // [msec]
    };
    char receive_buffer[3];
    int send_n = pimpl->scip_.receive(receive_buffer, 3, RB_Timeout);
    if (send_n != 3) {
      return false;
    }
    if (strncmp(receive_buffer, "RB\n", 3)) {
      return false;
    }
  }
  disconnect();

  return true;
}


bool UrgDevice::versionLines(std::vector<std::string>& lines)
{
  if (! isConnected()) {
    return false;
  }
  return pimpl->scip_.versionLines(lines);
}


size_t UrgDevice::scanMsec(void) const
{
  int scan_rpm = pimpl->parameters_.scan_rpm;
  return (scan_rpm <= 0) ? 1 : (1000 * 60 / scan_rpm);
}


size_t UrgDevice::minDistance(void) const
{
  return pimpl->parameters_.distance_min;
}


size_t UrgDevice::maxDistance(void) const
{
  return pimpl->parameters_.distance_max;
}


size_t UrgDevice::maxRange(void) const
{
  // 1 は、parameters_ が未初期化のときに new long [0] しないための処置
  return max(pimpl->parameters_.area_max, 1);
}


#if 0
void UrgDevice::setTimestamp(long timestamp)
{
  static_cast<void>(timestamp);
  // !!!
}
#endif


void UrgDevice::setCaptureTimes(size_t times)
{
  if (times > 99) {
    times = 0;
  }
  pimpl->capture_times_ = times;
}


void UrgDevice::setCaptureRange(size_t begin_index, size_t end_index)
{
  // データ取得を停止する
  pimpl->stop();

  pimpl->capture_begin_ = begin_index;
  pimpl->capture_end_ = end_index;
}


void UrgDevice::setSkipLines(size_t skip_lines)
{
  if ((skip_lines > 0) && (skip_lines <= 99)) {
    pimpl->capture_skip_lines_ = skip_lines;
  }
}


void UrgDevice::setRequestMode(CaptureMode mode)
{
  pimpl->capture_mode_ = mode;
}


void UrgDevice::requestData(void)
{
  pimpl->requestData();
}


bool UrgDevice::receiveData(std::vector<long>& data, long* timestamp)
{
  if (! isConnected()) {
    pimpl->error_message_ = "not connected.";
    return false;
  }

  bool ret = pimpl->scip_.receiveData(data, timestamp);
  if (! ret) {
    pimpl->error_message_ = pimpl->scip_.what();
    return false;
  }

  if (timestamp) {
    // タイムスタンプが 24 bit の msec 経過しても １巡しないようにする
    if ((*timestamp >= 0) && (*timestamp < pimpl->pre_timestamp_)) {
      pimpl->base_timestamp_ += 1 << 24;
    }
    pimpl->pre_timestamp_ = *timestamp;
    *timestamp += pimpl->base_timestamp_;
  }

  return ret;
}


size_t UrgDevice::rad2index(double radian) const
{
  return pimpl->rad2index(radian);
}


double UrgDevice::index2rad(size_t index) const
{
  int index_from_front = index - pimpl->parameters_.area_front;
  return index_from_front * (2.0 * M_PI) / pimpl->parameters_.area_total;
}


void UrgDevice::laserOn(void)
{
  // !!!
}


void UrgDevice::laserOff(void)
{
  // !!!
}
