#ifndef CAMERA_ROBOT_CALIBRATION_H
#define CAMERA_ROBOT_CALIBRATION_H

#pragma once
#include <opencv2/opencv.hpp>
#include <libs/robot/robot_defs.h>
#include <fstream>

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

inline void Camera_Robot_Calibration(ri::Staubli& robot, interfaceBALLUFF::Camera& front_camera, interfaceBALLUFF::Camera& side_camera, tcpc::sParameters& parameters,
                                            calibration::stereo_camera cameras_matrix, Logger& log, uint8_t N=10, uint8_t moving_range=20){
    if(!robot.isEstablished()){
        log<<("Connection with Robot controller NOT established");
        throw std::runtime_error("Connection with Robot controller NOT established");
    }

    if(!robot.atHome()) robot.goHome();

    //modify speed for accuraccy
    robot.setSpeed(10);

    ri::Point point;
    cv::Mat grayMatF;
    cv::Mat grayMatS;
    double offset_X = 0;
    double diff_X = 0;
    double offset_Y = 0;
    double diff_Y = 0;
    double offset_Z = 0;

    int format = front_camera.get_Format_px();

    try {
        //X_coordinates
        for (int i=0;i<N;i++) {
            ri::Point point = parameters.camera_robot.position;
            robot.setPosition(point);

            cv::Mat front_frame = front_camera.readImage(format,-1,true).image;
            cv::Mat side_frame = side_camera.readImage(format,-1,true).image;

            grayMatF = tcpc::toGray(front_frame);
            grayMatS = tcpc::toGray(side_frame);
            cv::Point start_pointF = tcpc::detect_ArUco_gravity(grayMatF, cameras_matrix.Front_cam.K_matrix, cameras_matrix.Front_cam.Dist_coeffs);
            cv::Point start_pointS = tcpc::detect_ArUco_gravity(grayMatS, cameras_matrix.Side_cam.K_matrix, cameras_matrix.Side_cam.Dist_coeffs);

            point = robot.getPosition();
            double tmp = point.x;
            point.x -= moving_range;
            robot.setPosition(point);

            front_frame = front_camera.readImage(format,-1,true).image;
            side_frame = side_camera.readImage(format,-1,true).image;

            grayMatF = tcpc::toGray(front_frame);
            grayMatS = tcpc::toGray(side_frame);
            cv::Point end_pointF = tcpc::detect_ArUco_gravity(grayMatF, cameras_matrix.Front_cam.K_matrix, cameras_matrix.Front_cam.Dist_coeffs);
            cv::Point end_pointS = tcpc::detect_ArUco_gravity(grayMatS, cameras_matrix.Side_cam.K_matrix, cameras_matrix.Side_cam.Dist_coeffs);

            diff_X += end_pointF.x - start_pointF.x;
            offset_X += end_pointS.x - start_pointS.x;
        }

        //Y_coordinates
        for (int i=0;i<N;i++) {
            ri::Point point = parameters.camera_robot.position;
            robot.setPosition(point);

            cv::Mat side_frame = side_camera.readImage(format,-1,true).image;
            cv::Mat front_frame = front_camera.readImage(format,-1,true).image;

            grayMatF = tcpc::toGray(front_frame);
            grayMatS = tcpc::toGray(side_frame);
            cv::Point start_pointF = tcpc::detect_ArUco_gravity(grayMatF, cameras_matrix.Front_cam.K_matrix, cameras_matrix.Front_cam.Dist_coeffs);
            cv::Point start_pointS = tcpc::detect_ArUco_gravity(grayMatS, cameras_matrix.Side_cam.K_matrix, cameras_matrix.Side_cam.Dist_coeffs);

            point = robot.getPosition();
            double tmp = point.y;
            point.y -= moving_range;
            robot.setPosition(point);

            front_frame = front_camera.readImage(format,-1,true).image;
            side_frame = side_camera.readImage(format,-1,true).image;

            grayMatF = tcpc::toGray(front_frame);
            grayMatS = tcpc::toGray(side_frame);
            cv::Point end_pointF = tcpc::detect_ArUco_gravity(grayMatF, cameras_matrix.Front_cam.K_matrix, cameras_matrix.Front_cam.Dist_coeffs);
            cv::Point end_pointS = tcpc::detect_ArUco_gravity(grayMatS, cameras_matrix.Side_cam.K_matrix, cameras_matrix.Side_cam.Dist_coeffs);

            diff_Y += end_pointS.x - start_pointS.x;
            offset_Y += end_pointF.x - start_pointF.x;
        }

        //Z_coordinates
        for (int i=0;i<N;i++) {
            robot.setPosition(parameters.camera_robot.position);

            cv::Mat side_frame = side_camera.readImage(format,-1,true).image;
            cv::Mat front_frame = front_camera.readImage(format,-1,true).image;

            grayMatF = tcpc::toGray(front_frame);
            grayMatS = tcpc::toGray(side_frame);
            cv::Point start_pointF = tcpc::detect_ArUco_gravity(grayMatF, cameras_matrix.Front_cam.K_matrix, cameras_matrix.Front_cam.Dist_coeffs);
            cv::Point start_pointS = tcpc::detect_ArUco_gravity(grayMatS, cameras_matrix.Side_cam.K_matrix, cameras_matrix.Side_cam.Dist_coeffs);

            point = robot.getPosition();
            point.z -= moving_range;
            robot.setPosition(point);

            front_frame = front_camera.readImage(format,-1,true).image;
            side_frame = side_camera.readImage(format,-1,true).image;

            grayMatF = tcpc::toGray(front_frame);
            grayMatS = tcpc::toGray(side_frame);
            cv::Point end_pointF = tcpc::detect_ArUco_gravity(grayMatF, cameras_matrix.Front_cam.K_matrix, cameras_matrix.Front_cam.Dist_coeffs);
            cv::Point end_pointS = tcpc::detect_ArUco_gravity(grayMatS, cameras_matrix.Side_cam.K_matrix, cameras_matrix.Side_cam.Dist_coeffs);

            offset_Z += end_pointF.y - start_pointF.y;
            offset_Z += end_pointS.y - start_pointS.y;
        }
    } catch (...) {
        log<<"Bad positions of the Robot-Camera calibration setted";
        throw std::runtime_error ("Bad positions of the Robot-Camera calibration");
    }

    offset_X /= N;
    offset_Y /= N;
    offset_Z /= 2*N;
    diff_X /= N;
    diff_Y /= N;

    parameters.camera_robot.coordinates_offset.x = moving_range/offset_X;
    parameters.camera_robot.coordinates_offset.y = moving_range/offset_Y;
    parameters.camera_robot.coordinates_offset.z = moving_range/offset_Z;
}

#endif // CAMERA_ROBOT_CALIBRATION_H
