/*!
  \file
  \brief コマンドの送受信

  \author Satofumi KAMIMURA

  $Id: CommandPacket.cpp 1407 2009-10-13 14:04:05Z satofumi $
*/

#include "CommandPacket.h"
#include "PacketAccess.h"
#include "Connection.h"
#include "delay.h"
#include <cassert>


using namespace qrk;
using namespace std;

namespace
{
  enum {
    DummyParameter = 0,
    DefaultTimeout = 100,       // [msec]

    DefaultStraightVelocity = 300,     // [mm/sec]
    DefaultRotateVelocity = 65536 >> 2 // [dir16/sec]
  };
}


struct CommandPacket::pImpl
{
  // !!! packet_handler.c と共通なのでくくる
  enum {
    PacketSize = 3,
    PacketType = 4,
    HeaderChecksum = 5,
    NumberOfData = 6,
    DataFirst = 7,

    WriteResponseSize = 8,
  };

  Connection* connection_;
  int timeout_;
  char packet_type_;
  vector<char> send_packet_;
  vector<unsigned char> receive_packet_;

  long mm_per_sec_;
  long dir16_per_sec_;


  pImpl(void)
    : connection_(NULL), timeout_(DefaultTimeout), packet_type_('\0'),
      mm_per_sec_(DefaultStraightVelocity),
      dir16_per_sec_(DefaultRotateVelocity)
  {
  }


  void beginWritePacket(void)
  {
    packet_type_ = 'W';
    createHeader();
  }


  void beginReadPacket(void)
  {
    packet_type_ = 'R';
    createHeader();
  }


  void createHeader(void)
  {
    send_packet_.clear();

    send_packet_.push_back('r');
    send_packet_.push_back('u');
    send_packet_.push_back('n');
    send_packet_.push_back(0x00); // packet size
    send_packet_.push_back(0x00); // packet type
    send_packet_.push_back(0x00); // header checksum
    send_packet_.push_back(0x00); // number of data

    send_packet_[PacketType] = packet_type_;
  }


  void endPacket(void)
  {
    send_packet_.push_back(0x00);
    size_t packet_size = send_packet_.size();
    assert(packet_size < 256);

    // パケットサイズを格納し、ヘッダとパケット全体のチェックサムを計算する
    send_packet_[PacketSize] = static_cast<unsigned char>(packet_size);
    setChecksum(HeaderChecksum);
    setChecksum(packet_size - 1);
  }


  void setChecksum(size_t checksum_index)
  {
    char sum = 0;
    for (size_t i = 0; i < checksum_index; ++i) {
      sum += send_packet_[i];
    }
    send_packet_[checksum_index] = sum;
  }


  bool sendPacket(void)
  {
    size_t packet_size = send_packet_.size();
    int n = connection_->send(&send_packet_[0], packet_size);
    if (static_cast<int>(packet_size) != n) {
      return false;
    } else {
      return true;
    }
  }


  bool waitResponse(void)
  {
    size_t receive_size = (packet_type_ == 'W') ?
      static_cast<size_t>(WriteResponseSize) : send_packet_.size();

    enum { BufferSize = 256 };
    char buffer[BufferSize];
    int n = connection_->receive(buffer, receive_size, timeout_);
    if (n <= 0) {
      return false;
    }

    // !!! 先頭の run を読み出してパケットの正当性を確認すべき

    // 書き込み応答は読み捨て、読み出し応答のみ処理する
    // !!!

    receive_packet_.clear();
    receive_packet_.reserve(n);
    for (int i = 0; i < n; ++i) {
      receive_packet_.push_back(buffer[i]);
    }

    return true;
  }
};


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


CommandPacket::~CommandPacket(void)
{
}


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


bool CommandPacket::version(unsigned char* major,
                            unsigned char* minor, unsigned char* micro)
{
  // バージョン情報の取得
  pimpl->beginReadPacket();
  robot_system_major_set(pimpl->send_packet_, DummyParameter);
  robot_system_minor_set(pimpl->send_packet_, DummyParameter);
  robot_system_micro_set(pimpl->send_packet_, DummyParameter);
  pimpl->endPacket();

  if (! pimpl->sendPacket()) {
    return false;
  }
  if (! pimpl->waitResponse()) {
    // 0.0.0 は無効なバージョンとする
    *major = 0;
    *minor = 0;
    *micro = 0;
    return false;
  }

  // パケットからの情報読み出し
  unsigned char* p = &pimpl->receive_packet_[0] + pImpl::DataFirst;
  *major = robot_system_major_get(p);
  *minor = robot_system_minor_get(p);
  *micro = robot_system_micro_get(p);

  return true;
}


bool CommandPacket::isStable(bool& stable)
{
  pimpl->beginReadPacket();
  robot_straight_is_stable_set(pimpl->send_packet_, DummyParameter);
  robot_rotate_is_stable_set(pimpl->send_packet_, DummyParameter);
  pimpl->endPacket();

  if (! pimpl->sendPacket()) {
    return false;
  }
  if (! pimpl->waitResponse()) {
    return false;
  }

  // パケットからの情報読み出し
  unsigned char* p = &pimpl->receive_packet_[0] + pImpl::DataFirst;
  char straight_stable = robot_straight_is_stable_get(p);
  char rotate_stable = robot_rotate_is_stable_get(p);

  stable = ((straight_stable * rotate_stable) > 0) ? true : false;

  return true;
}


bool CommandPacket::position(long mm[], unsigned short* dir16)
{
  // 位置情報の取得
  pimpl->beginReadPacket();
  robot_position_mm_0_set(pimpl->send_packet_, DummyParameter);
  robot_position_mm_1_set(pimpl->send_packet_, DummyParameter);
  robot_position_dir16_set(pimpl->send_packet_, DummyParameter);
  pimpl->endPacket();

  if (! pimpl->sendPacket()) {
    return false;
  }
  if (! pimpl->waitResponse()) {
    return false;
  }

  // パケットからの情報読み出し
  unsigned char* p = &pimpl->receive_packet_[0] + pImpl::DataFirst;
  mm[0] = robot_position_mm_0_get(p);
  mm[1] = robot_position_mm_1_get(p);
  *dir16 = robot_position_dir16_get(p);

  return true;
}


bool CommandPacket::stop(void)
{
  pimpl->beginWritePacket();
  robot_system_mode_set(pimpl->send_packet_, NormalControl);
  robot_straight_target_velocity_set(pimpl->send_packet_, 0);
  robot_rotate_target_velocity_set(pimpl->send_packet_, 0);

  pimpl->endPacket();

  if (! pimpl->sendPacket()) {
    return false;
  }
  if (! pimpl->waitResponse()) {
    return false;
  }

  return true;
}


bool CommandPacket::setWheelVelocity(int id, long mm_per_sec)
{
  if ((id != 0) && (id != 1)) {
    return false;
  }

  pimpl->beginWritePacket();
  robot_system_mode_set(pimpl->send_packet_, DirectControl_wheel);
  if (id == 0) {
    robot_direct_wheel_0_target_velocity_set(pimpl->send_packet_,
                                             mm_per_sec << FOLLOW_SHIFT);
  } else {
    robot_direct_wheel_1_target_velocity_set(pimpl->send_packet_,
                                             mm_per_sec << FOLLOW_SHIFT);
  }
  pimpl->endPacket();

  if (! pimpl->sendPacket()) {
    return false;
  }
  if (! pimpl->waitResponse()) {
    return false;
  }

  return true;
}


bool CommandPacket::setStraightVelocity(long mm_per_sec)
{
  pimpl->mm_per_sec_ = mm_per_sec;
  pimpl->beginWritePacket();

  // !!! mm_per_sec を登録して他の移動コマンドでも使うようにする
  robot_straight_target_velocity_set(pimpl->send_packet_,
                                     pimpl->mm_per_sec_ << FOLLOW_SHIFT);

  pimpl->endPacket();

  if (! pimpl->sendPacket()) {
    return false;
  }
  if (! pimpl->waitResponse()) {
    return false;
  }

  return true;
}


bool CommandPacket::straightVelocity(long* mm_per_sec)
{
  pimpl->beginReadPacket();
  // !!!
  pimpl->endPacket();

  if (! pimpl->sendPacket()) {
    return false;
  }
  if (! pimpl->waitResponse()) {
    return false;
  }

  // パケットからの情報読み出し
  unsigned char* p = &pimpl->receive_packet_[0] + pImpl::DataFirst;
  // !!!
  (void)p;
  (void)mm_per_sec;

  return true;
}


bool CommandPacket::maxStraightVelocity(long* mm_per_sec)
{
  pimpl->beginReadPacket();
  // !!!
  pimpl->endPacket();

  if (! pimpl->sendPacket()) {
    return false;
  }
  if (! pimpl->waitResponse()) {
    return false;
  }

  // パケットからの情報読み出し
  unsigned char* p = &pimpl->receive_packet_[0] + pImpl::DataFirst;
  // !!!
  (void)p;
  (void)mm_per_sec;

  return true;
}


bool CommandPacket::setRotateVelocity(long dir16)
{
  (void)dir16;

  // !!!

  return false;
}


bool CommandPacket::rotateVelocity(long* dir16)
{
  (void)dir16;

  // !!!

  return false;
}


bool CommandPacket::followLine(long x, long y, unsigned short dir16)
{
  pimpl->beginWritePacket();
  robot_system_mode_set(pimpl->send_packet_, NormalControl);
  robot_path_mode_set(pimpl->send_packet_, Line);
  robot_path_reset_offset_set(pimpl->send_packet_, 1);
  robot_follow_position_mm_0_set(pimpl->send_packet_, -x);
  robot_follow_position_mm_1_set(pimpl->send_packet_, -y);
  robot_follow_position_dir16_set(pimpl->send_packet_, -dir16);

  // !!! 定数を置き換えること
  robot_straight_target_velocity_set(pimpl->send_packet_,
                                     pimpl->mm_per_sec_ << FOLLOW_SHIFT);
  robot_rotate_target_velocity_set(pimpl->send_packet_,
                                   pimpl->dir16_per_sec_ << FOLLOW_SHIFT);

  // !!! 目標速度
  // !!! 加速度
  // !!! 追従時の曲率

  pimpl->endPacket();

  if (! pimpl->sendPacket()) {
    return false;
  }
  if (! pimpl->waitResponse()) {
    return false;
  }

  return true;
}


bool CommandPacket::stopLine(long x, long y, unsigned short dir16)
{
  // !!! followLine と共通な箇所はくくる

  pimpl->beginWritePacket();
  robot_system_mode_set(pimpl->send_packet_, NormalControl);
  robot_path_mode_set(pimpl->send_packet_, Line_stop);
  robot_path_reset_offset_set(pimpl->send_packet_, 1);
  robot_follow_position_mm_0_set(pimpl->send_packet_, -x);
  robot_follow_position_mm_1_set(pimpl->send_packet_, -y);
  robot_follow_position_dir16_set(pimpl->send_packet_, -dir16);

  // !!! 定数を置き換えること
  robot_straight_target_velocity_set(pimpl->send_packet_,
                                     pimpl->mm_per_sec_ << FOLLOW_SHIFT);
  robot_rotate_target_velocity_set(pimpl->send_packet_,
                                   pimpl->dir16_per_sec_ << FOLLOW_SHIFT);

  // !!! 目標速度
  // !!! 加速度
  // !!! 追従時の曲率

  pimpl->endPacket();

  if (! pimpl->sendPacket()) {
    return false;
  }
  if (! pimpl->waitResponse()) {
    return false;
  }

  return true;
}


bool CommandPacket::spin(long dir16)
{
  // !!! 回転の回数に 65536 より大きな値を指定すると、正常に動作しない

  pimpl->beginWritePacket();
  robot_system_mode_set(pimpl->send_packet_, NormalControl);
  robot_path_mode_set(pimpl->send_packet_, Rotation);
  robot_path_rotate_direction_set(pimpl->send_packet_, dir16);
  robot_rotate_target_velocity_set(pimpl->send_packet_,
                                   pimpl->dir16_per_sec_ << FOLLOW_SHIFT);
  pimpl->endPacket();

  if (! pimpl->sendPacket()) {
    return false;
  }
  if (! pimpl->waitResponse()) {
    return false;
  }

  return true;
}


bool CommandPacket::rotate(long dir16)
{
  pimpl->beginWritePacket();
  robot_system_mode_set(pimpl->send_packet_, NormalControl);
  robot_path_mode_set(pimpl->send_packet_, Rotation_stop);
  robot_path_rotate_direction_set(pimpl->send_packet_, dir16);
  robot_rotate_target_velocity_set(pimpl->send_packet_,
                                   pimpl->dir16_per_sec_ << FOLLOW_SHIFT);
  // !!! 加速度
  pimpl->endPacket();

  if (! pimpl->sendPacket()) {
    return false;
  }
  if (! pimpl->waitResponse()) {
    return false;
  }

  return true;
}


bool CommandPacket::followCircle(long x, long y, long radius)
{
  pimpl->beginWritePacket();
  robot_system_mode_set(pimpl->send_packet_, NormalControl);
  robot_path_mode_set(pimpl->send_packet_, Circle);
  robot_follow_position_mm_0_set(pimpl->send_packet_, -x);
  robot_follow_position_mm_1_set(pimpl->send_packet_, -y);
  robot_path_circle_radius_set(pimpl->send_packet_, radius);

  // !!! 目標速度
  // !!! 加速度

  robot_straight_target_velocity_set(pimpl->send_packet_,
                                     pimpl->mm_per_sec_ << FOLLOW_SHIFT);
  robot_rotate_target_velocity_set(pimpl->send_packet_,
                                   pimpl->dir16_per_sec_ << FOLLOW_SHIFT);


  pimpl->endPacket();

  if (! pimpl->sendPacket()) {
    return false;
  }
  if (! pimpl->waitResponse()) {
    return false;
  }

  return true;
}
