#include "VrConvert.h"

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

#include "IVrUtils.h"
#include "SimpleIni.h"

std::ostream& operator<<(std::ostream& os, const SVzNL3DPoint& sPoint) 
{
    os  << "3DPoint {x: " << sPoint.x << ", y: " << sPoint.y << ", z: " << sPoint.z << "}";
    return os;
}

// 获取标定矩阵个数
int CVrConvert::GetClibMatrixCount(const char* sFileName)
{
    CSimpleIniA ini;
    SI_Error rc = ini.LoadFile(sFileName);
    if(SI_OK != rc) return 0;
    
    return ini.GetLongValue("CommInfo", "nExistMatrixNum", 0);
}

bool CVrConvert::LoadClibMatrix(const char* sFileName,  const char* sIdent, const char* sKey,  double dMatrix[16])
{
    // 读取配置文件
    CSimpleIniA ini;

    // 加载文件并解析
    SI_Error rc = ini.LoadFile(sFileName);

    if(SI_OK != rc) return false;

    for(int i = 0 ; i < 16; i++)
    {
        char sTmpKey[64] = {0};
#ifdef _WIN32
        sprintf_s(sTmpKey, "%s_%d", sKey, i);
#else
        sprintf(sTmpKey, "%s_%d", sKey, i); 
#endif
        dMatrix[i] = ini.GetDoubleValue(sIdent, sTmpKey, i % 5 == 0 ? 1 : 0);
    }

    return true;
}

// 存储矩阵
bool CVrConvert::SaveClibMatrix(const char* sFileName, const char* sIdent, const char* sKey,  double dMatrix[16])
{
    CSimpleIniA ini;
    SI_Error rc = ini.LoadFile(sFileName);  
    if (rc != SI_OK) return false;

    for(int i = 0 ; i < 16; i++)
    {
        char sTmpKey[64] = {0};
#ifdef _WIN32
        sprintf_s(sTmpKey, "%s_%s_%d", sIdent, sKey, i);
#else
        sprintf(sTmpKey, "%s_%s_%d", sIdent, sKey, i);
#endif
        ini.SetDoubleValue(sIdent, sTmpKey, dMatrix[i]);
    }
    ini.SaveFile(sFileName);
    return true;
}

// 手眼标定
bool CVrConvert::CalibEyeToRobot(SVzNL3DPoint* pEye3DPoint, SVzNL3DPoint* pRobot3DPoint, const int nNum)
{
    if(nNum <= 0) return false;
    
#if 0
    Eigen::Matrix4d calibration_matrix = Eigen::Matrix4d::Identity();
    for(int i = 0; i < nNum; i++)
    {
        Eigen::Matrix<double, 4, 1>  eye, robot;
        eye << pEye3DPoint[i].x << pEye3DPoint[i].y << pEye3DPoint[i].z << 1;
        robot << pRobot3DPoint[i].x << pRobot3DPoint[i].y << pRobot3DPoint[i].z << 1;

        calibration_matrix += (eye.inverse() * robot);
    }
    // 求取平均值作为最终的手眼标定矩阵
    calibration_matrix /= nNum;

    std::cout << "Hand-eye calibration matrix:\n" << calibration_matrix << std::endl;
#endif
    return true;
}

bool CVrConvert::EyeToRobot(const SVzNL3DPoint sEyePoint, SVzNL3DPoint& sRobotPoint, const double clib[16])
{
    bool bRet = true;

    // Define the original arm coordinates (1x6)
    Eigen::Matrix<double, 4, 1> eyecoordinates;
    eyecoordinates << sEyePoint.x, sEyePoint.y, sEyePoint.z, 1;
#if 0
    Eigen::Matrix<double, 3, 1> eyeposture;
    eyeposture << sEyePoint.roll, sEyePoint.pitch, sEyePoint.yaw;
#endif
    // Define the transformation matrix (4x4)
    Eigen::Matrix4d transformation_matrix;
    // Fill in the values of the transformation matrix (example values)
    transformation_matrix << clib[0],   clib[1],    clib[2],    clib[3],
                             clib[4],   clib[5],    clib[6],    clib[7],
                             clib[8],   clib[9],    clib[10],   clib[11],
                             clib[12],  clib[13],   clib[14],   clib[15];

    Eigen::Matrix<double, 4, 1> robotcoordinates;
    robotcoordinates = transformation_matrix * eyecoordinates;

    sRobotPoint.x = robotcoordinates(0, 0);
    sRobotPoint.y = robotcoordinates(1, 0);
    sRobotPoint.z = robotcoordinates(2, 0);

#if 0
    Eigen::Matrix3d oriMatrix = transformation_matrix.block<3, 3>(0, 0);
    Eigen::Matrix<double, 3, 1> robotposture;
    robotposture = oriMatrix * eyeposture;
    sRobotPoint.roll = robotposture(0, 0);
    sRobotPoint.pitch = robotposture(1, 0);
    sRobotPoint.yaw = robotposture(2, 0);
#endif
    return bRet;
}