#ifndef ROBOT_DEFS_H
#define ROBOT_DEFS_H

#pragma once
#include <Eigen/Dense>
#include <opencv2/opencv.hpp>

namespace ri{

struct Point{
    double x, y , z, rx, ry, rz;

    Point() = default;

    Point(double x_mm, double y_mm, double z_mm, double rx_deg, double ry_deg, double rz_deg) :
                    x(x_mm), y(y_mm), z(z_mm), rx(rx_deg), ry(ry_deg), rz(rz_deg) { }

    friend std::ostream& operator<<(std::ostream& stream, Point const& Point){
        stream << "x: " << Point.x << "mm, y: " << Point.y << "mm, z: " << Point.z << "mm, "
               << "rx: " << Point.rx << "°, ry: " << Point.ry << "°, rz: " << Point.rz << "°";
        return stream;
    }

    Point(cv::Mat matrix){
        cv::Rect r(0,0,3,3);
        cv::Mat Rmat = matrix(r).clone();

        cv::Mat Rmat_transposed;
        cv::transpose(Rmat, Rmat_transposed);
        cv::Mat shouldBeIdentity = Rmat_transposed * Rmat;
        cv::Mat I = cv::Mat::eye(3,3, shouldBeIdentity.type());
        if(cv::norm(I, shouldBeIdentity) >= 1e-6){
            throw;
        }

       float sy = sqrt(Rmat.at<double>(0,0) * Rmat.at<double>(0,0) +  Rmat.at<double>(1,0) * Rmat.at<double>(1,0) );

       bool singular = sy < 1e-6; // If

       if(!singular){
            this->rx = atan2(Rmat.at<double>(2,1) , Rmat.at<double>(2,2));
            this->ry = atan2(-Rmat.at<double>(2,0), sy);
            this->rz = atan2(Rmat.at<double>(1,0), Rmat.at<double>(0,0));
        }else{
            this->rx = atan2(-Rmat.at<double>(1,2), Rmat.at<double>(1,1));
            this->ry = atan2(-Rmat.at<double>(2,0), sy);
            this->rz = 0;
        }
        this->rx *= (180/CV_PI);
        this->ry *= (180/CV_PI);
        this->rz *= (180/CV_PI);
        this->x = matrix.at<double>(0, 3);
        this->y = matrix.at<double>(1, 3);
        this->z = matrix.at<double>(2, 3);
    }

    cv::Mat toMatrix_OnlyCoordinates(){
        cv::Mat coordinates = cv::Mat_<float>(3,1);
        coordinates.at<float>(0) = this->x;
        coordinates.at<float>(1) = this->y;
        coordinates.at<float>(2) = this->z;
        return coordinates;
    }

    void fromMatrix_OnlyCoordinates(cv::Mat onlyCoordinates_Matrix){
        cv::Mat coordinates = cv::Mat_<float>(3,1);
        this->x = onlyCoordinates_Matrix.at<float>(0);
        this->y = onlyCoordinates_Matrix.at<float>(1);
        this->z = onlyCoordinates_Matrix.at<float>(2);
    }

    cv::Mat toMatrix(){
        cv::Mat result = cv::Mat_<double>::eye(4,4);
        cv::Vec3d theta;
            theta[0] = this->rx * (CV_PI/180);
            theta[1] = this->ry * (CV_PI/180);
            theta[2] = this->rz * (CV_PI/180);

        // Calculate rotation about x axis
        cv::Mat RotX = (cv::Mat_<double>(3,3) <<
            1,              0,              0,
            0,  cos(theta[0]),  -sin(theta[0]),
            0,  sin(theta[0]),  cos(theta[0])
        );

        // Calculate rotation about y axis
        cv::Mat RotY = (cv::Mat_<double>(3,3) <<
             cos(theta[1]),  0, sin(theta[1]),
                         0,  1,             0,
            -sin(theta[1]),  0, cos(theta[1])
        );

        // Calculate rotation about z axis
        cv::Mat RotZ = (cv::Mat_<double>(3,3) <<
            cos(theta[2]),  -sin(theta[2]), 0,
            sin(theta[2]),   cos(theta[2]), 0,
                        0,               0, 1
        );

        cv::Mat R = (RotZ * RotY * RotX);

        cv::Mat tmp = result(cv::Rect(0,0,3,3));
        R.copyTo(tmp);

        result.at<double>(0, 3) = this->x;
        result.at<double>(1, 3) = this->y;
        result.at<double>(2, 3) = this->z;
        return result;
    }

    bool operator!=(const Point value){
        bool result = false;
        result = x == value.x;
        result &= y == value.y;
        result &= z == value.z;
        result &= rx == value.rx;
        result &= ry == value.ry;
        result &= rz == value.rz;
        return result;
    }

    Eigen::Matrix4d matrix() const {
                double degtorad = std::atan(1) / 45.0;
                Eigen::Matrix4d coord_transform; // transform camera 3D space to robot 3D space
                coord_transform <<
                    0,  1,  0,  0,
                   -1,  0,  0,  0,
                    0,  0,  1,  0,
                    0,  0,  0,  1;
                Eigen::Affine3d r(
                    Eigen::AngleAxisd(rx * degtorad, Eigen::Vector3d::UnitX()) *
                    Eigen::AngleAxisd(ry * degtorad, Eigen::Vector3d::UnitY()) *
                    Eigen::AngleAxisd(rz * degtorad, Eigen::Vector3d::UnitZ())
                );
                Eigen::Affine3d t(Eigen::Translation3d(x, y, z));
                return (t * r).matrix() * coord_transform;
            }

    operator Eigen::Matrix4d() const {
        return this->matrix();
    }

    Point(Eigen::Matrix4d mat) {
        double radtodeg = 45.0 / std::atan(1);
        Eigen::Matrix4d inv_coord_transform; // transform robot 3D space to camera 3D space
        inv_coord_transform <<
            0, -1,  0,  0,
            1,  0,  0,  0,
            0,  0,  1,  0,
            0,  0,  0,  1;

        Eigen::Matrix4d tr_mat = mat * inv_coord_transform;

        x = tr_mat(0, 3);
        y = tr_mat(1, 3);
        z = tr_mat(2, 3);
        rx = tr_mat.block<3,3>(0,0).eulerAngles(0, 1, 2)(0) * radtodeg;
        ry = tr_mat.block<3,3>(0,0).eulerAngles(0, 1, 2)(1) * radtodeg;
        rz = tr_mat.block<3,3>(0,0).eulerAngles(0, 1, 2)(2) * radtodeg;
    }

};//Point


enum eRobot_TOOL{
    TOOL0 = 0, FLANGE = 0,
    TOOL1 = 1, GRIPPER = 1,
    TOOL2 = 2,
    TOOL3 = 3,
    TOOL4 = 4,
    TOOL5 = 5,
    TOOL6 = 6,
    TOOL7 = 7
};//eRobot_TOOL


enum eRobot_FRAME{
    FRAME0 = 0, WORLD = 0,
    FRAME1 = 1,
    FRAME2 = 2,
    FRAME3 = 3,
    FRAME4 = 4,
    FRAME5 = 5,
    FRAME6 = 6,
    FRAME7 = 7
};//eRobot_FRAME

}//namespace_ri


#endif // ROBOT_DEFS_H
