#include <iostream>
#include <cmath>
#include <typeinfo>
using namespace std;

// eigen
#include <vector>
#include <Eigen/Core>
#include <Eigen/Geometry>

// NumCpp
#include "NumCpp.hpp"

int main() {
//    Eigen::Vector3d test (0.2, 0.3, 0.1);
//    test.normalize();
    //小罗布1的旋转向量
    nc::NdArray<double> v1 = {0.35, 0.2, 0.3, 0.1};
    nc::NdArray<double> b = v1.sum()-0.35;

    for(int i=1;i<4;i++){
        v1[i] = v1[i]/b[0];
    }
//    for(auto it = a.begin(); it < a.end(); ++i){}
    std::cout << v1 << std::endl;
    Eigen::AngleAxisd rotation_v1 (v1[0]*M_PI, Eigen::Vector3d (v1[1], v1[2], v1[3]));
    Eigen::Matrix3d rotation_m1 = Eigen::Matrix3d::Identity();
    rotation_m1 = rotation_v1.toRotationMatrix();
    //小罗布2的旋转向量
    nc::NdArray<double> v2 = {-0.5, 0.4, -0.1, 0.2};
    nc::NdArray<double> c = nc::abs(v2).sum()-0.5;

    for(int i=1;i<4;i++){
        v2[i] = v2[i]/c[0];
    }
//    for(auto it = a.begin(); it < a.end(); ++i){}
    std::cout << v2 << std::endl;
    Eigen::AngleAxisd rotation_v2 (v2[0]*M_PI, Eigen::Vector3d (v2[1], v2[2], v2[3]));
    Eigen::Matrix3d rotation_m2 = Eigen::Matrix3d::Identity();
    rotation_m1 = rotation_v2.toRotationMatrix();

    //小罗布1的旋转矩阵和欧式变换矩阵

    Eigen::Isometry3d T1 = Eigen::Isometry3d::Identity();
    T1.rotate(rotation_m1);
    T1.pretranslate(Eigen::Vector3d (0.3, 0.1, 0.1));
    Eigen::Vector3d Tw = T1*(Eigen::Vector3d (0.5, 0, 0.2));

    //小罗布2的先转矩阵和欧式变换矩阵
    Eigen::Isometry3d T2 = Eigen::Isometry3d::Identity();
    T2.rotate(rotation_m2);
    T2.pretranslate(Eigen::Vector3d (-0.1, 0.5, 0.3));
    T2 = T2.inverse();
    Eigen::Vector3d Pc2 = T2 * Tw;
    cout << Pc2 << endl;

    return 0;
}