#ifndef MAIN_ALGORITHM_H
#define MAIN_ALGORITHM_H

#pragma once
#include <QThread>
#include <QMutex>

#include "libs/camera/camera.h"
#include "libs/camera_calibration/camera_calibration.h"
#include "libs/robot/staubli.h"
#include "libs/tcpc/tcpc_defs.h"
#include "libs/cv/mat_methods.h"
#include "libs/util/json_handler.h"

class Main_Algorithm : public QThread
{
    Q_OBJECT
public:
    explicit Main_Algorithm(QObject *parent = 0);

    //Main_Algorithm();

    ~Main_Algorithm();

    void run();

    void stop();

    void setFront_camera(interfaceBALLUFF::Camera& camera);

    void setSide_camera(interfaceBALLUFF::Camera& camera);

    void setRobot(ri::Staubli& robot);

    void setSettings(tcpc::sParameters parameters);

    void getTCP(ri::Point& point);

    bool stopFlag = false;

    QString getLog();

signals:
    void ShowFrames(camera::frame, camera::frame);

    void ShowMarked(QImage,QImage);

    void ShowStats(QString,QString);

    void NumberChanged(int);

    void MeasurementProgressChanged(double);

    void CalculationProgressChanged(double);


private:
    bool isRunnable();

    bool detectGripper();

    void robotToDefaultPosition();

    void takeFrames(cv::Mat& front_frame, cv::Mat& side_frame);

    ri::Point estimateRobotPosition(cv::Point frontTCP, cv::Point sideTCP, bool& flag);

    void MarkFrame(cv::Mat& frame, cv::Point position, cv::Point req_position);

    void emitMarkedFrames(cv::Mat front_frame, cv::Point front_TCP, cv::Mat side_frame, cv::Point side_TCP);

    void processing_1st_position(cv::Point& frontTCP, cv::Point& sideTCP);

    void processing_2nd_position(cv::Point& frontTCP, cv::Point& sideTCP);

    void processing_3rd_position(cv::Point& frontTCP, cv::Point& sideTCP);

    Eigen::Matrix4d pointToMatrix(ri::Point pt);

    double calibQuality(std::vector<ri::Point> calibPositions);

    Eigen::Vector3d computeReferencePoint(std::vector<ri::Point> calibPositions, ri::Point gripper);

    double gripperOffsetLoss(std::vector<ri::Point> calibPositions, Eigen::Matrix4d gripperMatrix);


    ri::Point estimateTCP(std::vector<ri::Point>detected_points);

    interfaceBALLUFF::Camera* front_camera = nullptr;
    interfaceBALLUFF::Camera* side_camera = nullptr;
    ri::Staubli* robot = nullptr;
    tcpc::sParameters settings;
    int format = -99;
    long duration = 0;
    ri::Point TCP;
    QString resLog;
};

#endif // MAIN_ALGORITHM_H
