#include "ObjectManipulationWindow.h"
#include "ProcessorThread.h"
#include <QMessageBox>
#include <QGraphicsPixmapItem>
#include <QLabel>
#include <QCloseEvent>
#include <QInputDialog>
#include <QDialog>

#include <string>
#include <sstream>
#include <iostream>

#include <opencv2/opencv.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#ifndef M_PI_2
#define M_PI_2 3.14159265358979323846/2
#endif

ObjectManipulationWindow::bin_button::bin_button(QString input) {
    QStringList params = split_with_escape(input);
    for (auto &p : params) {
        int i = p.indexOf('=');
        if (i > 0) {
            QString key = p.mid(0,i);
            QString value = p.mid(i+1);
            if (key == "id") {
                id = value.toInt();
            } else if (key == "width") {
                width = value.toFloat();
            } else if (key == "photo") {
                if (value == "true" || value == "1") {
                    photo = true;
                } else {
                    photo = false;
                }
            } else if (key == "label") {
                label = value;
            } else if (key == "icon") {
                icon = value;
            } else if (key == "capacity") {
                capacity = value.toInt();
                if (capacity == -1) {
                    current_capacity = 1;
                } else {
                    current_capacity = capacity;
                }
            } else if (key == "color") {
                color = value;
            } else {
                qWarning() << "bin_button(): unknown key:" << key;
            }
        } else {
            qWarning() << "bin_button(): wrong token:" << p;
        }
        
    }
}

QString ObjectManipulationWindow::bin_button::to_string() const {
    QString label_temp = label;
    label_temp.replace(",", "\\,").replace("\\", "\\\\");
    QString icon_temp = icon;
    icon_temp.replace(",", "\\,").replace("\\", "\\\\");
    QString retval = QString("id=%1,width=%2,photo=%3,label=%4,icon=%5,color=%6,capacity=%7").arg(id).arg(width).arg(photo).arg(label_temp).arg(icon_temp).arg(color).arg(capacity);
    return retval;
}




ObjectManipulationWindow::ObjectManipulationWindow(QWidget *parent, Qt::WindowFlags flags) {
    ui.setupUi(this);
    
    QGraphicsScene *scene = new QGraphicsScene(this);
    ui.graphicsView_preview->setScene(scene);
    ui.graphicsView_preview->show();
    scene = new QGraphicsScene(this);
    ui.graphicsView_object->setScene(scene);
    ui.graphicsView_object->show();
    
    set_interface_size(80);
    
    m_view_scroll_step = 20;
    m_zoom_in_out_factor = 0.3;
    m_initial_rotation = 0;
    m_rotation_step = M_PI_2;
    
    buttons_.push_back({{1, 1}, ui.toolButton_rotate_left});
    buttons_.push_back({{1, 1}, ui.toolButton_rotate_right});
    buttons_.push_back({{1, 1}, ui.toolButton_zoom_in});
    buttons_.push_back({{1, 1}, ui.toolButton_zoom_out});
    buttons_.push_back({{1, 1}, ui.toolButton_view_up});
    buttons_.push_back({{1, 1}, ui.toolButton_view_down});
    buttons_.push_back({{1, 1}, ui.toolButton_view_left});
    buttons_.push_back({{1, 1}, ui.toolButton_view_right});
    buttons_.push_back({{1, 1}, ui.toolButton_view_reset});
    buttons_.push_back({{2, 1}, ui.toolButton_cancel});
    buttons_.push_back({{1, 1}, ui.toolButton_refresh});

    ui.graphicsView_preview->setMouseTracking(true);
    
    connect(&bin_buttons_group_, SIGNAL(buttonClicked(int)), this, SLOT(bin_button_clicked(int)));

    connect(ui.graphicsView_preview, SIGNAL(left_release(QPointF)), this, SLOT(preview_clicked(QPointF)));
    connect(ui.graphicsView_preview, SIGNAL(mouse_move(QPointF)), this, SLOT(preview_mouse_moved(QPointF)));
    connect(ui.graphicsView_object, SIGNAL(left_release(QPointF)), this, SLOT(look_at(QPointF)));
    
    connect(ui.graphicsView_object, SIGNAL(right_press(QPointF)), this, SLOT(zoom_rect_start(QPointF)));
    connect(ui.graphicsView_object, SIGNAL(right_release(QPointF)), this, SLOT(zoom_rect_stop(QPointF)));

}

ObjectManipulationWindow::~ObjectManipulationWindow() {

}

void ObjectManipulationWindow::resizeEvent(QResizeEvent *e /* event */) {

    QWidget::resizeEvent(e);

    //QRect rec = ui.graphicsView_object->geometry();
    //manipulation_preview_size_ = cv::Size(rec.width(), rec.height());
    update_interface();
}

int ObjectManipulationWindow::interface_size() const {
    return m_interface_size;
}

void ObjectManipulationWindow::set_interface_size(int interface_size) {
    m_interface_size = interface_size;
    
    for (auto &button_info : buttons_) {
        QVector2D s = button_info.first;
        QToolButton* button = button_info.second;
        //        button->setMinimumSize(s.x()*size, s.y()*size);
        //        button->setMaximumSize(s.x()*size, s.y()*size);
        button->setFixedSize(s.x()*interface_size, s.y()*interface_size);
        button->setIconSize(QSize(0.75*interface_size,0.75*interface_size));

        QFont f = button->font();
        QFontMetrics metrics(f);
        //f.setPixelSize(interface_size*0.9/button->text().length()*s.x());
        
        //qDebug() << button->text() << ;
        
        //        float horizontal_font_ratio = (float)metrics.width(button->text()) / (s.x()*size);
        //        qDebug() << button->text();
        //        qDebug() << horizontal_font_ratio;
        //        //float vertical_font_ratio = (float)metrics.height() / (s.y()*size);
        //
        //        //if (horizontal_font_ratio < vertical_font_ratio) {
        //        f.setPixelSize((float)size / horizontal_font_ratio / 2);
        //        qDebug() << "Font:" << (float)size / horizontal_font_ratio / 2;
        //} else {
        //    f.setPixelSize(vertical_font_ratio * size);
        //}
        button->setFont(f);
    }
    /*if(m_show_complex_gestures_help)
    {

        QPixmap image1(":/icons/icons/legenda2.png");
        ui.label->setPixmap(image1.scaled(3*interface_size,600,Qt::KeepAspectRatio));
        QPixmap image2(":/icons/icons/legenda3.png");
        //ui.->setPixmap(image2.scaled(1.5*interface_size,600,Qt::KeepAspectRatio));
    }*/
    QList<QAbstractButton*> bin_btns = bin_buttons_group_.buttons();
    
    int i = 0;
    for (auto &button : bin_btns) {
        button->setFixedHeight(interface_size);
        button->setFixedWidth(interface_size*bin_buttons_[i].width);
        button->setIconSize(QSize(interface_size*bin_buttons_[i].width, interface_size));
        
        if (!button->text().isEmpty()) {
            QFont f = button->font();
            QFontMetrics metrics(f);
            f.setPixelSize(bin_buttons_[i].width*interface_size*0.9/button->text().length());
            button->setFont(f);
        }
        i++;
    }
    update_interface();
}

int ObjectManipulationWindow::initial_rotation() const {
    return m_initial_rotation * 180.0 / M_PI;
}

void ObjectManipulationWindow::set_initial_rotation(int initial_rotation) {
    m_initial_rotation = (float)initial_rotation * M_PI / 180.0;
}

int ObjectManipulationWindow::rotation_step() const {
    return m_rotation_step * 180.0 / M_PI;
}

void ObjectManipulationWindow::set_rotation_step(int rotation_step) {
    m_rotation_step = (float)rotation_step * M_PI / 180.0;
}

float ObjectManipulationWindow::view_scroll_step() const {
    return m_view_scroll_step;
}

void ObjectManipulationWindow::set_view_scroll_step(float view_scroll_step) {
    m_view_scroll_step = view_scroll_step;
}

float ObjectManipulationWindow::zoom_in_out_factor() const {
    return m_zoom_in_out_factor;
}

void ObjectManipulationWindow::set_zoom_in_out_factor(float zoom_in_out_factor) {
    m_zoom_in_out_factor = zoom_in_out_factor;
}

QString ObjectManipulationWindow::bin_descriptors() const {
    QString bin_descriptors_string;
    for (int i = 0; i < bin_buttons_.size(); i++) {
        bin_descriptors_string.append(bin_buttons_[i].to_string());
        if (i < bin_buttons_.size() - 1) {
            bin_descriptors_string.append(";");
        }
    }
    return bin_descriptors_string;
}

void ObjectManipulationWindow::set_bin_descriptors(const QString &bin_descriptors) {
    bin_buttons_.clear();
    
    QList<QAbstractButton*> bin_btns = bin_buttons_group_.buttons();
    for (auto &button : bin_btns) {
        bin_buttons_group_.removeButton(button);
        ui.horizontalLayout_bin_buttons->removeWidget(button);
        button->deleteLater();
    }
    
    QStringList bin_list = bin_descriptors.split(';');
    for (auto &b : bin_list) {
        if (!b.isEmpty()) {
            bin_buttons_.push_back(bin_button(b));
        }
    }
    int i = 0;
    for (auto &bin_btn : bin_buttons_) {
        QToolButton* btn = new QToolButton;
        
        if (!bin_btn.color.isEmpty()) {
            btn->setStyleSheet("background-color: " + bin_btn.color);
        }
        
        if (!bin_btn.icon.isEmpty()) {
            //btn->setIconSize(QSize(128, 128));
            btn->setIcon(QIcon(bin_btn.icon));
            //btn->setFixedSize(64, 64);
        } else if (bin_btn.label.isEmpty()) {
            //btn->setText("Bin " + QString::number(bin_btn.id));
        } else {
            btn->setText(bin_btn.label);
        }
        bin_buttons_group_.addButton(btn, i);
        ui.horizontalLayout_bin_buttons->addWidget(btn);
        i++;
    }
    set_interface_size(m_interface_size);
}

QString ObjectManipulationWindow::image_file() const {
    return m_image_file;
}

void ObjectManipulationWindow::set_image_file(const QString &image_file) {
    m_image_file = image_file;
    /* TODO */
    cv::Mat temp = cv::imread(image_file.toStdString());
    if (temp.data) {
        update_mode_ = update_mode_pause;
        
        current_image_ = temp;
        current_objects_.clear();
        sObject temp;
        temp.image_x = current_image_.cols/2;
        temp.image_y = current_image_.rows/2;
        temp.bounding_box = cv::RotatedRect(cv::Point2f(current_image_.cols/2,current_image_.rows/2), cv::Size2f(current_image_.cols, current_image_.rows), 0);
        current_objects_.push_back(temp);
        
        selected_object_ = 0;
        
        manipulation_mode();
    }
}
QString ObjectManipulationWindow::object_list_file() const {
    return m_object_list_file;
}
void ObjectManipulationWindow::set_object_list_file(const QString &object_list_file) {
    m_object_list_file = object_list_file;
    /* TODO */
    qDebug() << "ObjectManipulationWindow::set_object_list_file(): TODO";
    
    /*QFile objects_file("objects.txt");
    if(objects_file.open(QIODevice::ReadOnly | QIODevice::Text)) {
        QTextStream objects_stream(&objects_file);
        while (1) {
            QString line = objects_stream.readLine();
            if (line.isEmpty()) {
                break;
            }
            sObject object(line.toStdString());
            current_objects_.push_back(object);
        }
        for (int i = 0; i < current_objects_.size(); i++) {
            std::string mask_filename = "object_mask_" + std::to_string(i) + ".png";
            current_objects_[i].mask = cv::imread(mask_filename);
        }
    }
    
    for (int i = 0; i < current_objects_.size(); i++) {
        std::cerr << current_objects_[i].to_string() << ", " << current_objects_[i].mask.cols << "x" << current_objects_[i].mask.rows << std::endl;
    }*/
}

void ObjectManipulationWindow::show_gestures_legend(int is_show)
{
    m_show_complex_gestures_help = is_show;
    if(m_show_complex_gestures_help)
    {

        QPixmap image1(":/icons/icons/legenda2.png");
        ui.label->setPixmap(image1.scaled(3*interface_size(),600,Qt::KeepAspectRatio));
        QPixmap image2(":/icons/icons/legenda3.png");
        ui.label_click_help->setPixmap(image2.scaled(1.5*interface_size(),600,Qt::KeepAspectRatio));
    }

}



void ObjectManipulationWindow::refresh() {
    update_mode_ = update_mode_single;
    
    update_object_selection_view();
}

void ObjectManipulationWindow::update_object_selection_view() {
    cv::Mat preview;
    current_image_.copyTo(preview);
    
    for (int i = 0; i < current_objects_.size(); i++) {
        std::string text_label = std::to_string(i);// + "\na=" + std::to_string(objects[i].bounding_box.angle);
        
        //putText(*(outputMat[0]), text_label, cv::Point(objects[idx].x, objects[idx].y), cv::FONT_HERSHEY_PLAIN, 1.0, cvScalar(0,0,0), 3);
        putText(preview, text_label, cv::Point(current_objects_[i].image_x-20, current_objects_[i].image_y+20), cv::FONT_HERSHEY_PLAIN, 2.0, cvScalar(255,255,255), 2);
        
        cv::Point2f rect_points[4];
        current_objects_[i].bounding_box.points(rect_points);
        
        cv::Scalar line_color = cv::Scalar(255,255,255);
        int line_width = 2;
        if (highlighted_object_ == i) {
            line_color = cv::Scalar(0,0,255);
            line_width = 3;
        }
        
        for(int j = 0; j < 4; j++) {
            line(preview, rect_points[j], rect_points[(j+1)%4], line_color, line_width, cv::LINE_AA );
        }
        
        line(preview, cv::Point(current_objects_[i].image_x, current_objects_[i].image_y), cv::Point(current_objects_[i].image_x+20*cos(current_objects_[i].rz()), current_objects_[i].image_y+20*sin(current_objects_[i].rz())), cv::Scalar(0,0,255), 2, cv::LINE_AA );
    }
    
    QImage img = Mat_to_QImage(preview);
    QPixmap item = QPixmap::fromImage(img);
    ui.graphicsView_preview->scene()->clear();
    ui.graphicsView_preview->scene()->addPixmap(item);
    ui.graphicsView_preview->scene()->setSceneRect(0, 0, preview.cols, preview.rows);

    redraw_object_selection_view();
}

cv::Mat ObjectManipulationWindow::get_object_image(cv::Size dst_size, float angle, float zoom, float view_offset_x, float view_offset_y) {
    
    cv::Mat dst_image;
    
    sObject obj = current_objects_[selected_object_];
    cv::Size object_size = obj.bounding_box.size;
    
    if (dst_size.width == 0 && dst_size.height == 0) {
        dst_size = cv::Size((object_size.width*fabs(cos(angle))+
                             object_size.height*fabs(sin(angle))),
                            (object_size.height*fabs(cos(angle))+
                             object_size.width*fabs(sin(angle))));
    }
    
    float zoom_abs = powf(2.0, zoom);
    
    cv::Mat src_transformation = cv::Mat::eye(3, 3, CV_32F);
    src_transformation.at<float>(0, 0) = cos(obj.rz());
    src_transformation.at<float>(0, 1) = -sin(obj.rz());
    src_transformation.at<float>(1, 0) = sin(obj.rz());
    src_transformation.at<float>(1, 1) = cos(obj.rz());
    src_transformation.at<float>(0, 2) = obj.bounding_box.center.x;
    src_transformation.at<float>(1, 2) = obj.bounding_box.center.y;
    
    cv::Point3f offset;
    offset.z = 1;
    
    cv::Point2f src_tri[3];
    cv::Point2f dst_tri[3];
    
    cv::Mat transformed_point = cv::Mat::zeros(3, 1, CV_32F);
    
    offset.x = -object_size.width/2.0;
    offset.y = -object_size.height/2.0;
    transformed_point = src_transformation * cv::Mat(offset);
    src_tri[0] = cv::Point2f(transformed_point.at<float>(0,0), transformed_point.at<float>(1,0));
    offset.x = object_size.width/2.0;
    offset.y = -object_size.height/2.0;
    transformed_point = src_transformation * cv::Mat(offset);
    src_tri[1] = cv::Point2f(transformed_point.at<float>(0,0), transformed_point.at<float>(1,0));
    offset.x = -object_size.width/2.0;
    offset.y = object_size.height/2.0;
    transformed_point = src_transformation * cv::Mat(offset);
    src_tri[2] = cv::Point2f(transformed_point.at<float>(0,0), transformed_point.at<float>(1,0));
    
    //std::cerr << src_tri[0] << "/" << src_tri[1] << "/" << src_tri[2] << std::endl;
    
    cv::Mat dst_transformation = cv::Mat::eye(3, 3, CV_32F);
    dst_transformation.at<float>(0, 0) = cos(angle);
    dst_transformation.at<float>(0, 1) = -sin(angle);
    dst_transformation.at<float>(1, 0) = sin(angle);
    dst_transformation.at<float>(1, 1) = cos(angle);
    dst_transformation.at<float>(0, 2) = 0;
    dst_transformation.at<float>(1, 2) = 0;
    
    float width_after_rotation = fabs(cos(angle)*object_size.width) + fabs(sin(angle)*object_size.height);
    float height_after_rotation = fabs(sin(angle)*object_size.width) + fabs(cos(angle)*object_size.height);
    
    float vertical_ratio = height_after_rotation / dst_size.height;
    float horizontal_ratio = width_after_rotation / dst_size.width;
    
    float ratio = max(vertical_ratio, horizontal_ratio);
    
    offset.x = -(float)object_size.width/2.0 / ratio * zoom_abs;
    offset.y = -(float)object_size.height/2.0 / ratio * zoom_abs;
    transformed_point = dst_transformation * cv::Mat(offset);
    dst_tri[0] = cv::Point2f(
                transformed_point.at<float>(0,0) + dst_size.width/2 + view_offset_x,
                transformed_point.at<float>(1,0) + dst_size.height/2 + view_offset_y);
    offset.x = (float)object_size.width/2.0 / ratio * zoom_abs;
    offset.y = -(float)object_size.height/2.0 / ratio * zoom_abs;
    transformed_point = dst_transformation * cv::Mat(offset);
    dst_tri[1] = cv::Point2f(
                transformed_point.at<float>(0,0) + dst_size.width/2 + view_offset_x,
                transformed_point.at<float>(1,0) + dst_size.height/2 + view_offset_y);
    offset.x = -(float)object_size.width/2.0 / ratio * zoom_abs;
    offset.y = (float)object_size.height/2.0 / ratio * zoom_abs;
    transformed_point = dst_transformation * cv::Mat(offset);
    dst_tri[2] = cv::Point2f(
                transformed_point.at<float>(0,0) + dst_size.width/2 + view_offset_x,
                transformed_point.at<float>(1,0) + dst_size.height/2 + view_offset_y);
    
    //std::cerr << dst_tri[0] << "/" << dst_tri[1] << "/" << dst_tri[2] << std::endl;
    
    cv::Mat transform = cv::getAffineTransform(src_tri, dst_tri);
    
    cv::warpAffine(current_image_, dst_image, transform, dst_size);
    
    return dst_image;
    
}

void ObjectManipulationWindow::update_object_manipulation_view() {
    cv::Mat manipulation_preview = get_object_image(manipulation_preview_size_, current_angle_, current_zoom_, current_offset_x_, current_offset_y_);
    
    QImage img = Mat_to_QImage(manipulation_preview);
    QPixmap item = QPixmap::fromImage(img);
    ui.graphicsView_object->scene()->clear();
    ui.graphicsView_object->scene()->addPixmap(item);
    ui.graphicsView_object->scene()->setSceneRect(0, 0, manipulation_preview.cols, manipulation_preview.rows);
    redraw_object_manipulation_view();
}

void ObjectManipulationWindow::on_toolButton_refresh_clicked() {
    refresh();
}

void ObjectManipulationWindow::on_toolButton_rotate_left_clicked() {
    current_angle_ -= m_rotation_step;
    update_object_manipulation_view();
    emit logEvent(interfaceLogEvents::rotate_left_selected,selected_object_, ui.stackedWidget->currentIndex(),QPointF(current_offset_x_,current_offset_y_),QPointF(manipulation_preview_size_.width,manipulation_preview_size_.height),current_angle_);
}

void ObjectManipulationWindow::on_toolButton_rotate_right_clicked() {
    current_angle_ += m_rotation_step;
    update_object_manipulation_view();
    emit logEvent(interfaceLogEvents::rotate_right_selected,selected_object_, ui.stackedWidget->currentIndex(),QPointF(current_offset_x_,current_offset_y_),QPointF(manipulation_preview_size_.width,manipulation_preview_size_.height),current_angle_);
}

void ObjectManipulationWindow::on_toolButton_zoom_in_clicked() {
    current_zoom_ += m_zoom_in_out_factor;
    current_offset_x_ *= powf(2.0, m_zoom_in_out_factor);
    current_offset_y_ *= powf(2.0, m_zoom_in_out_factor);
    update_object_manipulation_view();
    emit logEvent(interfaceLogEvents::zoom_in_selected,selected_object_, ui.stackedWidget->currentIndex(),QPointF(current_offset_x_,current_offset_y_),QPointF(manipulation_preview_size_.width,manipulation_preview_size_.height),current_angle_);
}

void ObjectManipulationWindow::on_toolButton_zoom_out_clicked() {
    current_zoom_ -= m_zoom_in_out_factor;
    current_offset_x_ /= powf(2.0, m_zoom_in_out_factor);
    current_offset_y_ /= powf(2.0, m_zoom_in_out_factor);
    update_object_manipulation_view();
    emit logEvent(interfaceLogEvents::zoom_out_selected,selected_object_, ui.stackedWidget->currentIndex(),QPointF(current_offset_x_,current_offset_y_),QPointF(manipulation_preview_size_.width,manipulation_preview_size_.height),current_angle_);
}

void ObjectManipulationWindow::on_toolButton_view_left_clicked() {
    current_offset_x_ -= m_view_scroll_step * manipulation_preview_size_.width;
    update_object_manipulation_view();
    emit logEvent(interfaceLogEvents::move_left_selected,selected_object_, ui.stackedWidget->currentIndex(),QPointF(current_offset_x_,current_offset_y_),QPointF(manipulation_preview_size_.width,manipulation_preview_size_.height),current_angle_);
}

void ObjectManipulationWindow::on_toolButton_view_right_clicked() {
    current_offset_x_ += m_view_scroll_step * manipulation_preview_size_.width;
    update_object_manipulation_view();
    emit logEvent(interfaceLogEvents::move_right_selected,selected_object_, ui.stackedWidget->currentIndex(),QPointF(current_offset_x_,current_offset_y_),QPointF(manipulation_preview_size_.width,manipulation_preview_size_.height),current_angle_);
}

void ObjectManipulationWindow::on_toolButton_view_up_clicked() {
    current_offset_y_ -= m_view_scroll_step * manipulation_preview_size_.height;
    update_object_manipulation_view();
    emit logEvent(interfaceLogEvents::move_up_selected,selected_object_, ui.stackedWidget->currentIndex(),QPointF(current_offset_x_,current_offset_y_),QPointF(manipulation_preview_size_.width,manipulation_preview_size_.height),current_angle_);
}

void ObjectManipulationWindow::on_toolButton_view_down_clicked() {
    current_offset_y_ += m_view_scroll_step * manipulation_preview_size_.height;
    update_object_manipulation_view();
    emit logEvent(interfaceLogEvents::move_down_selected,selected_object_, ui.stackedWidget->currentIndex(),QPointF(current_offset_x_,current_offset_y_),QPointF(manipulation_preview_size_.width,manipulation_preview_size_.height),current_angle_);
}

void ObjectManipulationWindow::on_toolButton_view_reset_clicked() {
    reset_view();
}

void ObjectManipulationWindow::on_toolButton_cancel_clicked() {
    preview_mode();
}

void ObjectManipulationWindow::reset_buttons() {
    set_bin_descriptors(bin_descriptors());
}

QGraphicsView* ObjectManipulationWindow::getPreviewWidget()
{
    return (QGraphicsView*)ui.graphicsView_preview;
}

QGraphicsView *ObjectManipulationWindow::getManipulationWidget()
{
    return ui.graphicsView_object;
}

void ObjectManipulationWindow::new_object_data(cv::Mat image, QVector<sObject> objects) {
    switch (update_mode_) {
    case update_mode_single:
        update_mode_ = update_mode_pause;
    case update_mode_live:
        image.copyTo(current_image_);
        if (interface_active_) {
            current_objects_ = objects;
        } else {
            current_objects_.clear();
        }
        update_object_selection_view();
        break;
    case update_mode_pause:
        break;
    }
}

void ObjectManipulationWindow::zoom_rect_widget(QRect rectangle) {
    QPointF tl = ui.graphicsView_object->widget_to_scene(rectangle.topLeft());
    QPointF br = ui.graphicsView_object->widget_to_scene(rectangle.bottomRight());
    
    zoom_rect(QRectF(tl, br));
}

void ObjectManipulationWindow::look_at(QPointF point) {
    current_offset_x_ -= point.x() - manipulation_preview_size_.width/2;
    current_offset_y_ -= point.y() - manipulation_preview_size_.height/2;
    update_object_manipulation_view();
    emit logEvent(interfaceLogEvents::look_at_selected,selected_object_, -1,QPointF(current_offset_x_,current_offset_y_),QPointF(manipulation_preview_size_.width,manipulation_preview_size_.height),current_angle_);
}

void ObjectManipulationWindow::zoom_rect(QRectF rectangle) {
    qDebug() << "zoom_rect(" << target_view_rect_ << ")";
    
    look_at(rectangle.center());
    
    float zoom = (float)manipulation_preview_size_.width / rectangle.width();
    float zoom_y = (float)manipulation_preview_size_.height / rectangle.height();
    if (zoom_y < zoom) {
        zoom = zoom_y;
    }
    current_zoom_ += log2(zoom);
    
    current_offset_x_ *= zoom;
    current_offset_y_ *= zoom;
    
    update_object_manipulation_view();
    
    emit logEvent(interfaceLogEvents::zoom_rect_selected,selected_object_,ui.stackedWidget->currentIndex(),QPointF(current_offset_x_,current_offset_y_),QPointF(manipulation_preview_size_.width,manipulation_preview_size_.height),current_angle_);
}
void ObjectManipulationWindow::zoom_rect_start(QPointF point) {
    target_view_rect_.setTopLeft(point);
}
void ObjectManipulationWindow::zoom_rect_stop(QPointF point) {
    target_view_rect_.setBottomRight(point);
    zoom_rect(target_view_rect_);
}

void ObjectManipulationWindow::update_interface() {
    switch (ui.stackedWidget->currentIndex()) {
        case 0:
            redraw_object_selection_view();
            break;
        case 1:
            redraw_object_manipulation_view();
            break;
    }
}

void ObjectManipulationWindow::bin_button_clicked(int button_i) {
    if(m_image_file=="") {
        if (interface_active_ && ui.stackedWidget->currentIndex() == 1) {
            qDebug() << "BIN" << bin_buttons_[button_i].id;
            if (selected_object_ >= 0 && selected_object_ < current_objects_.size()) {
                
                if (bin_buttons_[button_i].current_capacity > 0) {
                    QList<QAbstractButton*> bin_btns = bin_buttons_group_.buttons();
                    if (bin_buttons_[button_i].photo) {
                        cv::Mat button_img = get_object_image(cv::Size(bin_buttons_[button_i].width*interface_size(), interface_size()), current_angle_);
                        
                        bin_btns[button_i]->setIconSize(QSize(button_img.cols-10, button_img.rows-10));
                        
                        bin_btns[button_i]->setIcon(QIcon(QPixmap::fromImage(Mat_to_QImage(button_img))));
                        
                        update_interface();
                        
                        qDebug() << "BUTTON PHOTO";
                    }
                    
                    qDebug() << "PICK AT:" << current_objects_[selected_object_].workspace_x  << current_objects_[selected_object_].workspace_y  << current_objects_[selected_object_].workspace_z << current_objects_[selected_object_].size_z << -(current_objects_[selected_object_].rz() + current_angle_);
                    emit pickObject(QVector3D(
                                              (float)current_objects_[selected_object_].workspace_x,
                                              current_objects_[selected_object_].workspace_y,
                                              current_objects_[selected_object_].workspace_z),
                                    current_objects_[selected_object_].size_z,
                                    -(current_objects_[selected_object_].rz() + current_angle_ + M_PI),
                                    bin_buttons_[button_i].id);
                    interface_active_ = false;
                    
                    
                    if (bin_buttons_[button_i].capacity != -1) {
                        bin_buttons_[button_i].current_capacity--;
                    }
                    
                    preview_mode();
                }
                
            }
            emit logEvent(interfaceLogEvents::bin_button_selected,selected_object_, bin_buttons_[button_i].id,QPointF(current_offset_x_,current_offset_y_),QPointF(manipulation_preview_size_.width,manipulation_preview_size_.height),current_angle_);
        }
    } else {
        QList<QAbstractButton*> bin_btns = bin_buttons_group_.buttons();
        //bin_btns[button_i]->setCheckable(true);
        //            bin_btns[button_i]->setChecked(!bin_btns[button_i]->isChecked());
        if(bin_buttons_[button_i].state==0)
        {
            bin_buttons_[button_i].state = 1;
            if(bin_buttons_[button_i].capacity==0)
            {
                bin_btns[button_i]->setStyleSheet("background-color: #FF03A8;");
            }
            else
            {
                bin_btns[button_i]->setStyleSheet("background-color: #54FF03;");
            }
        }
        else
        {
            bin_buttons_[button_i].state = 0;
            bin_btns[button_i]->setStyleSheet("background-color: #fefbd8;");
        }
        emit logEvent(interfaceLogEvents::bin_button_toggled,bin_buttons_[button_i].capacity|bin_buttons_[button_i].state*16, bin_buttons_[button_i].id,QPointF(current_offset_x_,current_offset_y_),QPointF(manipulation_preview_size_.width,manipulation_preview_size_.height),current_angle_);
    }
}

void ObjectManipulationWindow::preview_clicked(QPointF point) {
    if (interface_active_) {
        for (int i = 0; i < current_objects_.size(); i++) {
            if (is_inside_rectangle(current_objects_[i].bounding_box, cv::Point2d(point.x(), point.y()))) {
                qDebug() << "PICKED" << i;
                selected_object_ = i;
                
                manipulation_mode();
                emit logEvent(interfaceLogEvents::manipulation_mode_selected,selected_object_, -1 ,QPointF(current_offset_x_,current_offset_y_),QPointF(manipulation_preview_size_.width,manipulation_preview_size_.height),current_angle_);
            }
        }
    }
}

void ObjectManipulationWindow::preview_mouse_moved(QPointF point) {
    int highlighted_object_new = -1;
    for (int i = 0; i < current_objects_.size(); i++) {
        if (is_inside_rectangle(current_objects_[i].bounding_box, cv::Point2d(point.x(), point.y()))) {
            highlighted_object_new = i;
            break;
        }
    }
    if (highlighted_object_new != highlighted_object_) {
        highlighted_object_ = highlighted_object_new;
        update_object_selection_view();
    }
}

void ObjectManipulationWindow::preview_mode() {
    update_mode_ = update_mode_live;
    highlighted_object_ = -1;
    selected_object_ = -1;
    ui.stackedWidget->setCurrentIndex(0);
    update_object_selection_view();
    emit logEvent(interfaceLogEvents::preview_mode_selected,selected_object_, ui.stackedWidget->currentIndex(),QPointF(current_offset_x_,current_offset_y_),QPointF(manipulation_preview_size_.width,manipulation_preview_size_.height),current_angle_);
}

void ObjectManipulationWindow::manipulation_mode() {
    if (selected_object_ >= 0 && selected_object_ < current_objects_.size()) {
        update_mode_ = update_mode_pause;
        ui.stackedWidget->setCurrentIndex(1);
        reset_view();
        update_object_manipulation_view();
    }
}
void ObjectManipulationWindow::commandStarted() {
    interface_active_ = false;
    emit logEvent(interfaceLogEvents::robot_move_started,selected_object_, ui.stackedWidget->currentIndex(),QPointF(current_offset_x_,current_offset_y_),QPointF(manipulation_preview_size_.width,manipulation_preview_size_.height),current_angle_);
}
void ObjectManipulationWindow::commandExecuted() {
    interface_active_ = true;
    emit logEvent(interfaceLogEvents::robot_move_finished,selected_object_, ui.stackedWidget->currentIndex(),QPointF(current_offset_x_,current_offset_y_),QPointF(manipulation_preview_size_.width,manipulation_preview_size_.height),current_angle_);
}
void ObjectManipulationWindow::robotEmergencyStopped() {
    interface_active_ = false;
    QMessageBox msgBox;
    msgBox.setText("Robot emergency stop");
    msgBox.setInformativeText("Please fix the robot first");
    msgBox.exec();
    interface_active_ = true;
}

