/*!
  \file
  \brief URG データの描画ウィジット

  \author Satofumi KAMIMURA

  $Id: UrgDrawWidget.cpp 1978 2012-04-05 16:23:43Z satofumi $

  \todo Intensity 描画の色を指定できるようにする
  \todo グリッドに数値と単位系を描画。画面左端と、画面下
  \todo 描画する色を変更できるようにする
*/

#include "UrgDrawWidget.h"
#include "RangeSensor.h"
#include "convert2d.h"
#include "ticks.h"
#include <QMouseEvent>
#include <deque>
#include <limits>

using namespace qrk;
using namespace std;

#if defined(MSC)
#undef min
#undef max
#endif


namespace
{
    const double DefaultPixelPerMm = 20.0;


    struct DrawData
    {
        vector<Point<long> > point_data;
        long timestamp;
        Color line_color;


        DrawData(vector<Point<long> >& point_data_,
                 long timestamp_, const Color& line_color_)
            : timestamp(timestamp_), line_color(line_color_)
        {
            swap(point_data, point_data_);
        }
    };
}


struct UrgDrawWidget::pImpl
{
    enum {
        MinimumWidth = 100,
        MinimumHeight = 100,

        PointLength = 30 * 1000,     // [mm]
    };

    UrgDrawWidget* widget_;

    Point<double> view_center_;
    double pixel_per_mm_;
    DrawMode draw_mode_;
    long draw_period_;
    long last_redraw_;
    long last_timestamp_;

    typedef deque<DrawData> DataArray;
    DataArray draw_data_;
    size_t width_;
    size_t height_;

    QColor clear_color_;
    Position<long> rotate_offset_;
    QPoint clicked_position_;
    bool now_pressed_;

    Color line_color_;


    pImpl(UrgDrawWidget* widget)
        : widget_(widget),
          pixel_per_mm_(DefaultPixelPerMm), draw_mode_(Lines),
          draw_period_(100), last_redraw_(0), last_timestamp_(0),
          width_(MinimumWidth), height_(MinimumHeight), clear_color_(Qt::white),
          rotate_offset_(Position<long>(0, 0, deg(90))), now_pressed_(false),
          line_color_(0.2f, 0.2f, 1.0f, 0.6f)
    {
    }


    void initializeForm(void)
    {
        widget_->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Expanding);
        widget_->setMinimumSize(MinimumWidth, MinimumHeight);

        widget_->setMouseTracking(true);
    }


    void removeOldData(long timestamp)
    {
        if (draw_data_.empty()) {
            return;
        }

        // タイムスタンプが異常な場合、全てのデータをクリアする
        if (draw_data_.back().timestamp > timestamp) {
            draw_data_.clear();
            return;
        }

        int index = 0;
        for (DataArray::iterator it = draw_data_.begin();
             it != draw_data_.end(); ++it, ++index) {
            long difference = timestamp - it->timestamp;
            if (difference < draw_period_) {
                break;
            }
        }

        if (index > 0) {
            DataArray::iterator it = draw_data_.begin();
            draw_data_.erase(it, it + index);
        }
    }


    void initializeGL(void)
    {
        widget_->qglClearColor(clear_color_);
        glEnable(GL_CULL_FACE);
        glEnable(GL_TEXTURE_2D);
        glEnable(GL_BLEND);
        glDisable(GL_DEPTH_TEST);

        glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
    }


    void resizeGL(int width, int height)
    {
        glViewport(0, 0, width, height);

        glMatrixMode(GL_PROJECTION);
        glLoadIdentity();

        double aspect = 1.0 * width / height;
        glOrtho(-1.0 * aspect, +1.0 * aspect, -1.0, +1.0,
                numeric_limits<int>::min(), numeric_limits<int>::max());

        glMatrixMode(GL_MODELVIEW);
        width_ = width;
        height_ = height;
    }


    void paintGL(void)
    {
        widget_->qglClearColor(clear_color_);
        glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
        glLoadIdentity();

        // 軸の描画
        drawAxis();

        // データの描画
        drawData();
    }


    void drawAxis(void)
    {
        double zoom_ratio = zoomRatio();
        Point<double> offset = drawOffset(zoom_ratio);

        // 補助線の描画
        drawSubAxis(offset);

        glColor3d(0.3, 0.3, 0.3);
        glBegin(GL_LINES);

        // Y 軸
        glVertex2d(-offset.x, -1.0);
        glVertex2d(-offset.x, +1.0);

        // X 軸
        double aspect = 1.0 * width_ / height_;
        glVertex2d(-aspect, -offset.y);
        glVertex2d(+aspect, -offset.y);

        // 背後 45 [deg] の補助線
        glVertex2d(-offset.x, -offset.y);
        glVertex2d((-PointLength * zoom_ratio) - offset.x,
                   (-PointLength * zoom_ratio) - offset.y);
        glVertex2d(-offset.x, -offset.y);
        glVertex2d((+PointLength * zoom_ratio) - offset.x,
                   (-PointLength * zoom_ratio) - offset.y);
        glEnd();

        // 円の補助線を描画
        for (int i = 0; i <= PointLength; i += 1000) {
            drawSubCircle(i, offset);
        }

        // 単位の表示
        // !!!
    }


    void drawSubCircle(int radius, const Point<double>& offset)
    {
        double pixel_radius = (radius / pixel_per_mm_) / (height_ / 2.0);

        glBegin(GL_LINE_STRIP);
        for (int theta = -45; theta <= (180 + 45); theta += 2) {

            double radian = M_PI * theta / 180.0;
            double x = pixel_radius * cos(radian);
            double y = pixel_radius * sin(radian);
            glVertex2d(x - offset.x, y - offset.y);
        }
        glEnd();
    }


    void drawSubAxis(const Point<double>& offset)
    {
        Point<int> center(width_ / 2, height_ / 2);
        Point<long> first(static_cast<int>((-center.x * pixel_per_mm_)
                                           - view_center_.x),
                          static_cast<int>((-center.y * pixel_per_mm_)
                                           - view_center_.y));
        Point<long> last(static_cast<int>((+center.x * pixel_per_mm_)
                                          - view_center_.x),
                         static_cast<int>((+center.y * pixel_per_mm_)
                                          - view_center_.y));

        const double threshold[] = { 4.0, 16.0, 32.0 };
        const int interval[] = { 10, 100, 1000 };
        const double color[] = { 0.8, 0.6, 0.3 };
        size_t n = sizeof(threshold) / sizeof(threshold[0]);
        for (size_t i = 0; i < n; ++i) {
            const double draw_threshold = threshold[i];
            if (pixel_per_mm_ > draw_threshold) {
                continue;
            }
            double alpha = 1.0 - (pixel_per_mm_ / draw_threshold);
            glColor4d(color[i], color[i], color[i], alpha);
            drawSubPointLine(interval[i], offset, first, last);
        }
    }


    void drawSubPointLine(int interval, const Point<double>& offset,
                          const Point<long>& first, const Point<long>& last)
    {
        glBegin(GL_LINES);
        for (int x = (first.x / interval) - 1;
             x < ((last.x / interval) + 1); ++x) {
            double draw_x = (interval * x / pixel_per_mm_) / (height_ / 2.0);
            glVertex2d(draw_x - offset.x, -1.0);
            glVertex2d(draw_x - offset.x, +1.0);
        }

        double aspect = 1.0 * width_ / height_;
        for (int y = (first.y / interval) - 1;
             y < ((last.y / interval) + 1); ++y) {
            double draw_y = (interval * y / pixel_per_mm_) / (height_ / 2.0);
            glVertex2d(-aspect, draw_y - offset.y);
            glVertex2d(+aspect, draw_y - offset.y);
        }
        glEnd();
    }


    void drawData(void)
    {
        // !!! 時間が経過するほど、薄い色で表示するようにする

        double zoom_ratio = zoomRatio();
        Point<double> offset = drawOffset(zoom_ratio);

        if (draw_mode_ == Lines) {
            // 中心から測定点への直線を描画
            glBegin(GL_LINES);
            for (DataArray::iterator line_it = draw_data_.begin();
                 line_it != draw_data_.end(); ++line_it) {

                Color& line_color = line_it->line_color;
                double diff = last_timestamp_ - line_it->timestamp;
                double alpha = 1.0 - (diff / draw_period_);
                glColor4d(line_color.r, line_color.g, line_color.b, alpha);

                vector<Point<long> >& line_data = line_it->point_data;;
                vector<Point<long> >::iterator end_it = line_data.end();
                for (vector<Point<long> >::iterator it = line_data.begin();
                     it != end_it; ++it) {

                    double x = it->x * zoom_ratio;
                    double y = it->y * zoom_ratio;
                    glVertex2d(-offset.x, -offset.y);
                    glVertex2d(x - offset.x, y - offset.y);
                }
            }
            glEnd();

        } else if (draw_mode_ == Polygon) {
            // !!! これを実現するには、convert2d 時に捨ててしまった情報が必要
            // !!! 実装方法を見直すべき
        }

        // 測定点の描画
        double base_size = 1.4;
        double mm_pixel = max(base_size / pixel_per_mm_, base_size);
        glPointSize(mm_pixel);

        glBegin(GL_POINTS);
        for (DataArray::iterator line_it = draw_data_.begin();
             line_it != draw_data_.end(); ++line_it) {

            // !!! 関数にする
            double diff = last_timestamp_ - line_it->timestamp;
            double alpha = 1.0 - (diff / draw_period_);
            glColor4d(1.0, 0.0, 0.0, alpha);

            vector<Point<long> >::iterator end_it = line_it->point_data.end();
            for (vector<Point<long> >::iterator it =
                     line_it->point_data.begin();
                 it != end_it; ++it) {

                double x = it->x * zoom_ratio;
                double y = it->y * zoom_ratio;
                glVertex2d(x - offset.x, y - offset.y);
            }
        }
        glEnd();
    }


    // [mm] -> [pixel]
    double zoomRatio(void)
    {
        return (1.0 / pixel_per_mm_ / (height_ / 2.0));
    }


    Point<double> drawOffset(double zoom_ratio)
    {
        return Point<double>(-view_center_.x * zoom_ratio,
                             -view_center_.y * zoom_ratio);
    }


    void setClickedPosition(QMouseEvent* event)
    {
        clicked_position_ = event->pos();
    }


    void updateZoomRatio(int steps)
    {
        double zoom = widget_->zoomRatio();
        zoom *= pow(1.1, steps);

        if (zoom > 500.0) {
            zoom = 500.0;
        } else if (zoom < 0.05) {
            zoom = 0.05;
        }
        widget_->setZoomRatio(zoom);
    }


    bool storeUrgData(int timestamp, vector<Point<long> >& point_data,
                      vector<Point<long> >* intensity_point_data = NULL)
    {
        removeOldData(timestamp);
        draw_data_.push_back(DrawData(point_data, timestamp, line_color_));
        if (intensity_point_data) {
            draw_data_.push_back(DrawData(*intensity_point_data, timestamp,
                                          Color(1.0f, 0.0f, 1.0f, 0.7f)));
        }
        last_timestamp_ = timestamp;

        return true;
    }
};


UrgDrawWidget::UrgDrawWidget(QWidget* parent)
    : QGLWidget(parent), pimpl(new pImpl(this))
{
    pimpl->initializeForm();
}


UrgDrawWidget::~UrgDrawWidget(void)
{
}


void UrgDrawWidget::clear(void)
{
    pimpl->draw_data_.clear();
    //redraw();
}


void UrgDrawWidget::initializeView(void)
{
    pimpl->pixel_per_mm_ = DefaultPixelPerMm;
    pimpl->view_center_ = Point<double>(0.0, 0.0);
    redraw();
}


void UrgDrawWidget::redraw(void)
{
    pimpl->last_redraw_ = ticks();
    updateGL();
}


bool UrgDrawWidget::setUrgData(qrk::RangeSensor* sensor)
{
    vector<long> data;
    vector<long> intensity_data;
    vector<Point<long> > intensity_point_data;
    long timestamp = 0;

    RangeCaptureMode capture_mode = sensor->captureMode();

    int n = 0;
    if (capture_mode != IntensityCapture) {
        n = sensor->capture(data, &timestamp);
    } else {
        n = sensor->captureWithIntensity(data, intensity_data, &timestamp);
    }
    if (n <= 0) {
        return false;
    }

    vector<Point<long> > point_data;
    qrk::convert2d(point_data, sensor, data, pimpl->rotate_offset_);
    if (capture_mode == IntensityCapture) {
        qrk::convert2d(intensity_point_data, sensor, intensity_data,
                       pimpl->rotate_offset_, numeric_limits<int>::max());
    }

    return pimpl->storeUrgData(timestamp, point_data, &intensity_point_data);
}


bool UrgDrawWidget::setUrgData(std::vector<long>& data,
                               const RangeSensor* sensor, long timestamp)
{
    vector<Point<long> > point_data;
    qrk::convert2d(point_data, sensor, data, pimpl->rotate_offset_);

    return pimpl->storeUrgData(timestamp, point_data, NULL);
}


bool UrgDrawWidget::setUrgData(std::vector<qrk::Point<long> >& data,
                               long timestamp)
{
    return pimpl->storeUrgData(timestamp, data, NULL);
}


bool UrgDrawWidget::setUrgIntensityData(std::vector<long>& data,
                                        const qrk::RangeSensor* sensor,
                                        long timestamp)
{
    vector<Point<long> > point_data;
    qrk::convert2d(point_data, sensor, data, pimpl->rotate_offset_,
                   numeric_limits<int>::max());

    pimpl->draw_data_.push_back(DrawData(point_data, timestamp,
                                         Color(1.0f, 0.0f, 1.0f, 0.7f)));
    return true;
}


bool UrgDrawWidget::setUrgIntensityData(vector<qrk::Point<long> >& data,
                                        long timestamp)
{
    pimpl->draw_data_.push_back(DrawData(data, timestamp,
                                         Color(1.0f, 0.0f, 1.0f, 0.7f)));
    return true;
}


void UrgDrawWidget::setDrawColor(const qrk::Color& line_color)
{
    pimpl->line_color_ = line_color;
}


void UrgDrawWidget::setDrawMode(DrawMode mode)
{
    pimpl->draw_mode_ = mode;
}


void UrgDrawWidget::setDrawPeriod(size_t msec)
{
    pimpl->draw_period_ = msec;
}


void UrgDrawWidget::setZoomRatio(double pixel_per_mm)
{
    pimpl->pixel_per_mm_ = pixel_per_mm;
    redraw();
}


double UrgDrawWidget::zoomRatio(void)
{
    return pimpl->pixel_per_mm_;
}


void UrgDrawWidget::updateZoomRatio(int steps)
{
    pimpl->updateZoomRatio(steps);
}


void UrgDrawWidget::setViewCenter(const Point<long>& point)
{
    pimpl->view_center_.x = point.x;
    pimpl->view_center_.y = point.y;
}


Point<long> UrgDrawWidget::viewCenter(void)
{
    return Point<long>(static_cast<long>(pimpl->view_center_.x),
                       static_cast<long>(pimpl->view_center_.y));
}


void UrgDrawWidget::initializeGL(void)
{
    pimpl->initializeGL();
}


void UrgDrawWidget::resizeGL(int width, int height)
{
    pimpl->resizeGL(width, height);
}


void UrgDrawWidget::paintGL(void)
{
    pimpl->paintGL();
}


void UrgDrawWidget::mousePressEvent(QMouseEvent* event)
{
    pimpl->now_pressed_ = true;
    pimpl->setClickedPosition(event);
}


void UrgDrawWidget::mouseMoveEvent(QMouseEvent* event)
{
    int x = event->x();
    int y = event->y();

    if (pimpl->now_pressed_) {
        int dx = x - pimpl->clicked_position_.x();
        int dy = y - pimpl->clicked_position_.y();

        pimpl->view_center_.x += dx * pimpl->pixel_per_mm_;
        pimpl->view_center_.y -= dy * pimpl->pixel_per_mm_;

        pimpl->setClickedPosition(event);

        // 最後の更新が MinimumRedrawInterval [msec] 以前だったなら、
        // 再描画を行う
        enum { MinimumRedrawInterval = 25 };
        if ((pimpl->last_redraw_ + MinimumRedrawInterval) < ticks()) {
            redraw();
        }
    }

    // カーソル位置の座標をシグナルで送信する
    if ((x < 0) || (x >= static_cast<int>(pimpl->width_)) ||
        (y < 0) || (y >= static_cast<int>(pimpl->height_))) {
        emit position(false, -1, -1);
        return;
    }

    int center_x = pimpl->width_ / 2;
    int center_y = pimpl->height_ / 2;
    int x_mm = static_cast<int>(((x - center_x) * pimpl->pixel_per_mm_)
                                - pimpl->view_center_.x);
    int y_mm = static_cast<int>((-(y - center_y) * pimpl->pixel_per_mm_)
                                - pimpl->view_center_.y);
    emit position(true, x_mm, y_mm);
}


void UrgDrawWidget::mouseReleaseEvent(QMouseEvent* event)
{
    static_cast<void>(event);

    pimpl->now_pressed_ = false;
    redraw();
}


void UrgDrawWidget::wheelEvent(QWheelEvent* event)
{
    int degrees = event->delta() / 8;
    int steps = degrees / 15;

    event->accept();
    updateZoomRatio(steps);
}
