/**
    05Robot - Computer Vision Module

    @author P.Kaczmarek,J.Tomczynski,T.Mankowski
    @version 1.0 2017
*/
#ifndef PROCESSOR_OBJECTEXTRACTION_H
#define PROCESSOR_OBJECTEXTRACTION_H

#include "Processor.h"

#include <opencv2/opencv.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/calib3d/calib3d.hpp>

#include <QDebug>
#include <QVector3D>

#include <sstream>
/** \addtogroup ComputerVisionModule
 *  @{
 */
#ifndef M_PI
    #define M_PI 3.14159265358979323846
#endif
using namespace std;


/**
 @brief Detected object
 
 Contains information about single object, both in image and physical coordinates.
 */
struct sObject {
    sObject() {
        
    }
    sObject(std::string str) {
        std::stringstream ss;
        ss.str(str);
        double rz_temp;
        std::string token;
        int i = 0;
        bool cont = true;
        while(cont && std::getline(ss, token, ',')) {
            switch(i) {
                case 0:
                    break;
                case 1:
                    image_x = QString::fromStdString(token).toDouble();
                    break;
                case 2:
                    image_y = QString::fromStdString(token).toDouble();
                    break;
                case 3:
                    image_area = QString::fromStdString(token).toDouble();
                    break;
                case 4:
                    workspace_x = QString::fromStdString(token).toDouble();
                    break;
                case 5:
                    workspace_y = QString::fromStdString(token).toDouble();
                    break;
                case 6:
                    workspace_z = QString::fromStdString(token).toDouble();
                    break;
                case 7:
                    size_x = QString::fromStdString(token).toDouble();
                    break;
                case 8:
                    size_y = QString::fromStdString(token).toDouble();
                    break;
                case 9:
                    size_z = QString::fromStdString(token).toDouble();
                    break;
                case 10:
                    bounding_box.center.x = QString::fromStdString(token).toDouble();
                    break;
                case 11:
                    bounding_box.center.y = QString::fromStdString(token).toDouble();
                    break;
                case 12:
                    bounding_box.size.width = QString::fromStdString(token).toDouble();
                    break;
                case 13:
                    bounding_box.size.height = QString::fromStdString(token).toDouble();
                    break;
                case 14:
                    rz_temp = QString::fromStdString(token).toDouble();
                    bounding_box.angle = rz_temp * 180.0 / M_PI;
                    break;
                default:
                    cont = false;
                    break;
            }
            i++;
        }
    }
    double image_x;
    double image_y;
    double image_area;
    double workspace_x;
    double workspace_y;
    double workspace_z;
    double size_x;
    double size_y;
    double size_z;
    double rz() {
        return bounding_box.angle / 180.0 * M_PI;
    }
    cv::RotatedRect bounding_box;
    cv::Mat mask;
    
    std::string to_string() {
        std::stringstream ss;
        ss << image_x <<  ", " << image_y << ", " << image_area << ", " << workspace_x << ", " << workspace_y << ", " << workspace_z << ", " << size_x << ", " << size_y << ", " << size_z << ", " << bounding_box.center.x << ", " << bounding_box.center.y << ", " << bounding_box.size.width << ", " << bounding_box.size.height << ", " << rz();
        return ss.str();
    }
};

Q_DECLARE_METATYPE(QVector<sObject>)
Q_DECLARE_METATYPE(cv::Mat)


/**
 @brief Object extraction image processor
 
 Performs object extraction based on an internal background model.
 
 Detects center of gravity of selected objects and performs rectangle fitting resulting in width, height and angle of extracted object.
 
 After receiving object coordinates in pixels through processSpecial, if an object is found at given coordinates, calculates physical properties, position and angle of object to pick and emits pickObject signal.
 
 */
class Processor_ObjectExtraction: public Processor
{
	Q_OBJECT
    /**
     @brief Half of window size applied to foreground extraction mask
     */
    ADD_PROPERTY(median_khalfsize, int)
    
    /**
     @brief Offset between lower-left chessboard corner crossing and workspace origin in x axis, in meters
     */
    ADD_PROPERTY(workspace_x_offset, double)
    /**
     @brief Offset between lower-left chessboard corner crossing and workspace origin in y axis, in meters
     */
    ADD_PROPERTY(workspace_y_offset, double)
    /**
     @brief Offset between lower-left chessboard corner crossing and workspace origin in z axis, in meters
     */
    ADD_PROPERTY(workspace_z_offset, double)
    /**
     @brief Workspace size along x axis, in meters
     */
    ADD_PROPERTY(workspace_x_size, double)
    /**
     @brief Workspace size along y axis, in meters
     */
    ADD_PROPERTY(workspace_y_size, double)
    /**
     @brief Picked object height, in meters
     */
    ADD_PROPERTY(object_height, double)
    /**
     @brief Minimum detected object area, in pixels
     */
    ADD_PROPERTY(minimum_object_area, int)
    /**
     @brief Maximum detected object area, in pixels
     */
    ADD_PROPERTY(maximum_object_area, int)
    /**
     @brief Background model filename
     */
    ADD_PROPERTY(background_filename, QString)
public:
    /**
     @brief Initializes default parameters
     */
    Processor_ObjectExtraction();
    /**
     @brief Takes a screenshot of current scene, and logs objects info
     */
    void screenshot(const QString& filename);
    
signals:
    /**
     Emitted after successful selection of an object to pick and place into a bin

     @param pointInMeters Position of object to pick in workspace coordinates (in meters)
     @param angle Angle of short side of fitted rectangle
     @param bin ID of bin to put the object into
     */
    void pickObject(QVector3D pointInMeters, float object_height, float angle, int bin);
    void object_data(cv::Mat image, QVector<sObject> objects);

private:
    cv::Point2d pixel_to_meters(cv::Point2d pixel, cv::Size img_size);
    
	enum Processor_ObjectExtraction_Mode {Processor_ObjectExtraction_Mode_GetBackground, Processor_ObjectExtraction_Mode_Normal, Processor_ObjectExtraction_Mode_Object_Selected};
	Processor_ObjectExtraction_Mode mode;
    
    cv::Ptr<cv::BackgroundSubtractor> pMOG2_;
    cv::Mat labels_, stats_, centroids_;
    int x_, y_;
    int timeout_ = 0;
    QString screenshotFilename_;
    bool screenshot_ = false; // TODO REMOVE
	
public:
    /**
     @brief Starts image processing
     
     @return 0 on success
     */
	int start();
    /**
     @brief Processes frames in input buffer
     
     @param t1 Cameras trigger time
     @param t2 Cameras acquisition finish time
     @return 0 on success
     */
    int process(int t1, int t2);
    /**
     @brief Processes special command
     
     Valid commands are:
     <br>get_background: calculates background based on next frame
     <br>save_background: saves current background model to a file specified by \ref background_filename
     <br>load_background: reads background model from a file specified by \ref background_filename
     <br>object_picked x y: if an object is detected at x, y pixel coordinates, emits pickObject on success
     
     @param s Command string
     @return 0 on success
     */
	int processSpecial(string s);
    /**
     @brief Stops image processing
     
     @return 0 on success
     */
	int stop();
    /**
     @brief Writes properties to XML stream and current background model to a file specified by \ref background_filename
     
     @param stream XML stream to write to
     */
    virtual void writeXml(QXmlStreamWriter& stream);
    /**
     @brief Reads properties from XML stream and current background model to a file specified by \ref background_filename
     
     @param stream XML stream to read from
     */
    virtual void readXml(QXmlStreamReader& stream);
    
};
/** @}*/
#endif
