/*!
  \file
  \brief URG Viewer Sample

  \author Satofumi KAMIMURA

  $Id: cUrgViewerWindow.cpp 1439 2009-10-22 02:09:34Z satofumi $
*/

#include "UrgViewerWindow.h"
#include "UrgDrawWidget.h"
#include "cConnectThread.h"
#include "ConnectionWidget.h"
#include "cCaptureSettingWidget.h"
#include "LengthViewWidget.h"
#include "urg_ctrl.h"
#include "RangeSensorParameter.h"
#include "RangeCaptureMode.h"
#include "UrgUsbCom.h"
#include "FindComPorts.h"
#include "ticks.h"
#include "Lock.h"
#include "LockGuard.h"
#include "log_printf.h"
#include "math_utils.h"
#include <QTimer>
#include <QShortcut>
#include <QMessageBox>
#include <QSettings>
#include <QCloseEvent>

using namespace qrk;
using namespace std;


namespace
{
  const char* Organization = "Hokuyo LTD.";
  const char* Application = "URG Viewer";
}


struct UrgViewerWindow::pImpl
{
  UrgViewerWindow* widget_;

  urg_t urg_;
  Lock urg_mutex_;

  UrgUsbCom urg_usb_;
  FindComPorts urg_finder_;

  cCaptureSettingWidget capture_setting_widget_;
  LengthViewWidget length_view_widget_;

  UrgDrawWidget urg_draw_widget_;
  ConnectionWidget connection_widget_;
  cConnectThread connect_thread_;
  bool draw_intensity_;

  QTimer capture_timer_;

  RangeSensorParameter parameter_;
  CaptureSettings* settings_;
  long first_timestamp_;

  bool set_length_view_;
  vector<int> draw_line_indexes_;
  vector<Point<long> > draw_line_points_;


  pImpl(int argc, char *argv[], UrgViewerWindow* widget)
    : widget_(widget), capture_setting_widget_(urg_),
      connect_thread_(urg_), draw_intensity_(false),
      settings_(NULL), first_timestamp_(0), set_length_view_(false)
  {
    urg_finder_.addBaseName("/dev/ttyACM");
    urg_finder_.addBaseName("/dev/tty.usbmodem");
    urg_finder_.addDriverName("URG Series USB Device Driver");
    urg_finder_.addDriverName("URG-X002 USB Device Driver");

    for (int i = 1; i < argc; ++i) {
      if (! strcmp("-i", argv[i])) {
        draw_intensity_ = true;
      }
    }

    if (draw_intensity_) {
      capture_setting_widget_.setIntensityMode();
    }
  }


  ~pImpl(void)
  {
    delete settings_;
  }


  void initializeForm(void)
  {
    // タイトルの更新
    widget_->setWindowTitle(tr("C URG Viewer"));

    // ウィジットの配置
    widget_->dummy_label_1_->hide();
    widget_->dummy_label_2_->hide();
    widget_->main_layout_->addWidget(&urg_draw_widget_);
    widget_->connect_layout_->addWidget(&connection_widget_);

    // メニュー項目
    connect(widget_->action_quit_, SIGNAL(triggered()),
            widget_, SLOT(close()));
    connect(widget_->action_about_, SIGNAL(triggered()),
            widget_, SLOT(aboutApplication()));
    connect(widget_->action_zoomIn_, SIGNAL(triggered()),
            widget_, SLOT(zoomLarger()));
    connect(widget_->action_zoomOut_, SIGNAL(triggered()),
            widget_, SLOT(zoomSmaller()));
    connect(widget_->action_capture_setting_, SIGNAL(triggered()),
            widget_, SLOT(showCaptureSettingWidget()));
    connect(widget_->action_length_view_, SIGNAL(triggered()),
            widget_, SLOT(showLengthViewWidget()));

    // URG デバイス
    connect(&capture_timer_, SIGNAL(timeout()),
            widget_, SLOT(captureHandler()));

    connect(&connection_widget_, SIGNAL(rescanRequest()),
            widget_, SLOT(rescanPressed()));
    connect(&connection_widget_,
            SIGNAL(connectRequest(bool, const std::string&)),
            widget_, SLOT(connectPressed(bool, const std::string&)));
    connect(&connect_thread_, SIGNAL(connected(bool)),
            widget_, SLOT(connectDevice(bool)));

    // ステータスバー表示
    connect(&urg_draw_widget_, SIGNAL(position(bool, long, long)),
            widget_, SLOT(updateStatusBar(bool, long, long)));

    // 取得間隔の表示
    connect(&urg_draw_widget_, SIGNAL(scansInterval(int)),
            widget_, SLOT(updateScansInterval(int)));

    // キー割り付け
    (void) new QShortcut(Qt::Key_Return, widget_, SLOT(initializeView()));
    (void) new QShortcut(Qt::Key_Less, widget_, SLOT(zoomSmaller()));
    (void) new QShortcut(Qt::Key_Comma, widget_, SLOT(zoomSmaller()));
    (void) new QShortcut(Qt::Key_Greater, widget_, SLOT(zoomLarger()));
    (void) new QShortcut(Qt::Key_Period, widget_, SLOT(zoomLarger()));

    // 外部 Widget からのシグナル
    connect(&capture_setting_widget_,
            SIGNAL(setCaptureSettings(const qrk::CaptureSettings&)),
            widget_, SLOT(receiveCaptureSettings(const qrk::CaptureSettings&)));
    connect(&capture_setting_widget_, SIGNAL(reconnectRequest()),
            widget_, SLOT(receiveReconnectRequest()));
    connect(&length_view_widget_, SIGNAL(lengthViewRequest()),
            widget_, SLOT(receiveLengthViewRequest()));

    connect(&capture_setting_widget_, SIGNAL(widgetClose(const std::string&)),
            widget_, SLOT(receiveWidgetClose(const std::string&)));
    connect(&length_view_widget_, SIGNAL(widgetClose(const std::string&)),
            widget_, SLOT(receiveWidgetClose(const std::string&)));
    connect(&length_view_widget_, SIGNAL(quit()), widget_, SLOT(close()));
    connect(&capture_setting_widget_, SIGNAL(quit()), widget_, SLOT(close()));

    connect(&length_view_widget_,
            SIGNAL(drawLineRequest(const std::vector<int>&)),
            widget_, SLOT(receiveDrawLine(const std::vector<int>&)));

    connect(widget_, SIGNAL(reconnectCompleted()),
            &capture_setting_widget_, SLOT(reconnectCompleted()));

    // キー割り付け
    (void) new QShortcut(Qt::CTRL + Qt::Key_W, widget_, SLOT(close()));

    widget_->rescanPressed();
  }


  void saveSettings(void)
  {
    QSettings settings(Organization, Application);

    settings.setValue("geometry", widget_->saveGeometry());
    settings.setValue("LengthView_geometry",
                      length_view_widget_.saveGeometry());
    settings.setValue("CaptureSetting_geometry",
                      capture_setting_widget_.saveGeometry());
  }


  void loadSettings(void)
  {
    QSettings settings(Organization, Application);

    widget_->restoreGeometry(settings.value("geometry").toByteArray());
    length_view_widget_.
      restoreGeometry(settings.value("LengthView_geometry").toByteArray());
    capture_setting_widget_.
      restoreGeometry(settings.value("CaptureSetting_geometry").toByteArray());
  }


  void connectDevice(bool connected)
  {
    connection_widget_.setConnected(connected);
    capture_setting_widget_.setConnected(connected);
    if (! connected) {
      // エラーメッセージのポップアップ表示
      log_printf("%s\n", urg_error(&urg_));
      QMessageBox::warning(widget_, tr("Connection error"), urg_error(&urg_));
      return;
    }

    // パラメータ情報が与えられていない場合、センサ情報を反映させる
    urg_parameter_t urg_parameter;
    urg_parameters(&urg_, &urg_parameter);
    parameter_.model = urg_parameter.sensor_type;
    parameter_.distance_min = urg_parameter.distance_min_;
    parameter_.distance_max = urg_parameter.distance_max_;
    parameter_.area_total = urg_parameter.area_total_;
    parameter_.area_min = urg_parameter.area_min_;
    parameter_.area_max = urg_parameter.area_max_;
    parameter_.area_front = urg_parameter.area_front_;
    parameter_.scan_rpm = urg_parameter.scan_rpm_;

    if (! settings_) {
      settings_ = initializeCaptureSettings(parameter_.area_min,
                                            parameter_.area_max);
      capture_setting_widget_.loadPressed();
    }

    size_t type_length = parameter_.model.find('(');
    string urg_type = parameter_.model.substr(0, type_length);
    bool cluster_enable = ! (draw_intensity_ && urg_type.compare("UTM-30LX"));
    if (draw_intensity_) {
      capture_setting_widget_.setIntensityMode(cluster_enable);
    }

    receiveReconnectRequest();
    length_view_widget_.setMinDistance(urg_minDistance(&urg_));

    // URG 設定
    urg_enableTimestampMode(&urg_);
    first_timestamp_ = urg_currentTimestamp(&urg_);
    urg_disableTimestampMode(&urg_);
    int scan_msec = urg_scanMsec(&urg_);

    // データの取得開始
    capture_timer_.setInterval(scan_msec / 2);
    capture_timer_.start();
  }


  CaptureSettings* initializeCaptureSettings(int first_index, int last_index)
  {
    CaptureSettings* settings = new CaptureSettings;
    settings->type = GD;
    settings->capture_first = first_index;
    settings->capture_last = last_index;
    settings->skip_lines = 1;
    settings->skip_frames = 0;
    settings->remain_times = 0;
    settings->data_byte = 3;

    return settings;
  }

  void receiveReconnectRequest(void)
  {
    capture_timer_.stop();
    urg_draw_widget_.clear();

#if 0
    // !!! 保留
    if (draw_intensity_) {
      urg_.setCaptureMode(IntensityCapture);

    } else if (settings_->type == MD) {
      urg_.setCaptureMode(AutoCapture);

    } else if (settings_->type == GD) {
      urg_.setCaptureMode(ManualCapture);
    }
#endif

    // !!! 取得時に指定するので、不要なはず
    //urg_.setCaptureRange(settings_->capture_first, settings_->capture_last);
    urg_setSkipLines(&urg_, settings_->skip_lines);

    if (settings_->type != GD) {
      urg_setSkipFrames(&urg_, settings_->skip_frames);
      urg_setCaptureTimes(&urg_, settings_->remain_times);
    }

    if (! urg_isConnected(&urg_)) {
      // 未接続ならば再開させない
      return;
    }

    capture_timer_.start();
  }


  void disconnectDevice(void)
  {
    // データの取得停止
    capture_timer_.stop();
    urg_draw_widget_.clear();
    capture_setting_widget_.setApplyEnabled(true);

    connection_widget_.setConnected(false);
    capture_setting_widget_.setConnected(false);
  }


  void captureHandler(void)
  {
    LockGuard guard(urg_mutex_);

    vector<long> data;
    vector<long> intensity_data;
    long timestamp = 0;

    // !!! まずは Intensity 以外で動作させる
    RangeCaptureMode capture_mode = ManualCapture; //urg_.captureMode();
    int n = 0;
    //if (capture_mode != IntensityCapture) {
    //n = urg_.capture(data, &timestamp);
    //fprintf(stderr, "capture (%d, %d)\n", settings_->capture_first, settings_->capture_last);
    urg_requestData(&urg_, URG_GD,
                    settings_->capture_first, settings_->capture_last);

    //} else {
    //n = urg_.captureWithIntensity(data, intensity_data, &timestamp);
    //}
    int data_max = urg_dataMax(&urg_);
    data.resize(data_max);
    n = urg_receiveData(&urg_, &data[0], data_max);
    if (n <= 0) {
      return;
    }
    data.resize(n);
    timestamp = urg_recentTimestamp(&urg_) - first_timestamp_;

    // setUrgData() を呼び出すとデータは空になるので、
    // ここで LengthView 用のデータ格納を行う
    if (set_length_view_) {
      set_length_view_ = false;

      length_view_widget_.setLength(data);
      if (capture_mode == IntensityCapture) {
        length_view_widget_.setIntensity(intensity_data);
      }
    }

    // 補助線の登録
    for (vector<int>::iterator it = draw_line_indexes_.begin();
         it != draw_line_indexes_.end(); ++it) {
      double radian = urg_index2rad(&urg_, *it) + (M_PI/2.0);
      int x = static_cast<int>(100 * 1000 * cos(radian));
      int y = static_cast<int>(100 * 1000 * sin(radian));
      draw_line_points_.push_back(Point<long>(x, y));
    }
    urg_draw_widget_.setDrawColor(Color(0.0, 1.0, 0.0, 1.0));
    urg_draw_widget_.setUrgData(draw_line_points_, timestamp);

    // 描画データの登録
#if 0
    // !!! 保留
    if (capture_mode == IntensityCapture) {
      urg_draw_widget_.setUrgIntensityData(intensity_data, &urg_, timestamp);
    }
#endif
    urg_draw_widget_.setDrawColor(Color(0.2, 0.2, 1.0, 0.6));

    //urg_draw_widget_.setDrawMode(UrgDrawWidget::Points);

    vector<Point<long> > points;
    for (int i = 0; i < n; ++i) {
#if 1
      // !!! エラーデータも描画してしまう
      long distance = data[i];
      double radian = urg_index2rad(&urg_, i) + (M_PI/2.0);
      long x = static_cast<long>(distance * cos(radian));
      long y = static_cast<long>(distance * sin(radian));
#else
      long x = i - urg_deg2index(&urg_, 0);
      long y = data[i] / 4;
#endif
      points.push_back(Point<long>(x, y));
    }
    urg_draw_widget_.setUrgData(points, timestamp);

    // 一定間隔での描画
    // !!! FPS を定義する
    // !!! 可能ならば、URG のタイムスタンプを利用する
    urg_draw_widget_.redraw();

    if (settings_->type != GD) {
      if ((settings_->remain_times > 0) &&
          (urg_remainCaptureTimes(&urg_) == 0)) {
        // 回数指定のデータ取得のときは、指定回数後に取得を終了する
        capture_timer_.stop();
        capture_setting_widget_.setApplyEnabled(true);
      }
    }
  }


  void setPlayConnection(void)
  {
    // 接続操作を disable にする
    connection_widget_.setEnabled(false);

    // 接続処理
    QTimer::singleShot(1000, widget_, SLOT(autoPlay()));
  }


  void activateWidget(QWidget* widget, QAction* action)
  {
    widget->show();
    widget->raise();
    action->setChecked(true);
  }
};


UrgViewerWindow::UrgViewerWindow(int argc, char *argv[])
  : pimpl(new pImpl(argc, argv, this))
{
  setupUi(this);
  pimpl->initializeForm();
  pimpl->loadSettings();
}


UrgViewerWindow::~UrgViewerWindow(void)
{
  pimpl->saveSettings();
}


void UrgViewerWindow::rescanPressed(void)
{
  vector<string> devices;
  pimpl->urg_finder_.find(devices);
  for (vector<string>::iterator it = devices.begin();
       it != devices.end(); ++it) {
    if (pimpl->urg_usb_.isUsbCom(it->c_str())) {
      *it = *it + " [URG]";
    }
  }
  pimpl->connection_widget_.setDevices(devices);
}


void UrgViewerWindow::connectPressed(bool connection,
                                     const string& device)
{
  LockGuard guard(pimpl->urg_mutex_);

  if (connection) {
    // URG との接続
    pimpl->connect_thread_.setConnectSettings(device, 115200);
    pimpl->connect_thread_.start();

  } else {
    // 切断
    pimpl->disconnectDevice();
    urg_disconnect(&pimpl->urg_);
  }
}


void UrgViewerWindow::connectDevice(bool connection)
{
  pimpl->connectDevice(connection);
}


void UrgViewerWindow::captureHandler(void)
{
  pimpl->captureHandler();
}


void UrgViewerWindow::initializeView(void)
{
  pimpl->urg_draw_widget_.initializeView();
}


void UrgViewerWindow::updateStatusBar(bool active, long x_mm, long y_mm)
{
  if (! active) {
    statusBar()->clearMessage();
    return;
  }

  QString message = QString("X: %1 [mm], Y: %2 [mm]").arg(x_mm).arg(y_mm);
  statusBar()->showMessage(message, 60 * 1000);
}


void UrgViewerWindow::updateScansInterval(int msec)
{
  scans_label_->setText(QString(tr("%1 [msec] (scans interval)")).arg(msec));
}


void UrgViewerWindow::aboutApplication(void)
{
  QMessageBox::about(this, tr("About C URG Viewer"),
                     tr("<h2>C URG Viewer ($Rev: 1439 $)</h2>"
                        "<p>Demo Application for URG sensor</p>"
                        "<p>Report bugs to "
                        "&lt;s-kamimura@hokuyo-aut.co.jp&gt;</p>"));
}


void UrgViewerWindow::autoPlay(void)
{
  pimpl->connect_thread_.start();
}


void UrgViewerWindow::zoomSmaller(void)
{
  pimpl->urg_draw_widget_.updateZoomRatio(+1);
}


void UrgViewerWindow::zoomLarger(void)
{
  pimpl->urg_draw_widget_.updateZoomRatio(-1);
}


void UrgViewerWindow::closeEvent(QCloseEvent* event)
{
  static_cast<void>(event);

  pimpl->capture_setting_widget_.hide();
  pimpl->length_view_widget_.hide();

  pimpl->capture_timer_.stop();

  LockGuard guard(pimpl->urg_mutex_);
  urg_disconnect(&pimpl->urg_);
  pimpl->connect_thread_.terminate();
}


void UrgViewerWindow::showCaptureSettingWidget(void)
{
  pimpl->activateWidget(&pimpl->capture_setting_widget_,
                        action_capture_setting_);
}


void UrgViewerWindow::receiveCaptureSettings(const CaptureSettings& settings)
{
  if (! pimpl->settings_) {
    int dummy = -1;
    pimpl->settings_ = pimpl->initializeCaptureSettings(dummy, dummy);
  }
  *pimpl->settings_ = settings;
}


void UrgViewerWindow::receiveReconnectRequest(void)
{
  pimpl->receiveReconnectRequest();
  emit reconnectCompleted();
}


void UrgViewerWindow::showLengthViewWidget(void)
{
  pimpl->activateWidget(&pimpl->length_view_widget_,
                        action_length_view_);
}


void UrgViewerWindow::receiveLengthViewRequest(void)
{
  pimpl->set_length_view_ = true;
}


void UrgViewerWindow::receiveWidgetClose(const string& widget_name)
{
  if (! widget_name.compare("CaptureSettingWidget")) {
    action_capture_setting_->setChecked(false);

  } else if (! widget_name.compare("LengthViewWidget")) {
    action_length_view_->setChecked(false);
  }
}


void UrgViewerWindow::receiveDrawLine(const vector<int>& indexes)
{
  pimpl->draw_line_indexes_ = indexes;
}
