#include "CartForceScan.h"

void CartForceScan::Init()
{
    // 导纳控制参数
    M.diagonal() << 0.01, 0.01, 0.01, 0.001, 0.001, 0.001;
    B.diagonal() << 10.0, 10.0, 10.0, 0.8, 0.8, 0.8;
    K.diagonal() << 50.0, 50.0, 5.0, 50.0, 50.0, 50.0;

    Md = M.toDenseMatrix();
    Bd = B.toDenseMatrix();
    Kd = K.toDenseMatrix();

    deltaFkminus1.setZero();
    deltaFkminus2.setZero();
    Vkminus1.setZero();
    Vkminus2.setZero();

    tool2Basekminus1 = Eigen::Matrix4d::Identity();
    first = true;
}

void CartForceScan::LoadData(WayPoint curWayPt, FTData curForce, WayPoint refWayPt)
{
    //扫查模式下需要当前的笛卡尔力，当前的笛卡尔位姿和参考目标笛卡尔位姿
    _curCart = curWayPt.cartPos;
    _curForce = curForce.CartFT;
    _refCart = refWayPt.cartPos;
}

void CartForceScan::GetActualTarget(WayPoint& wayPoint)
{
    try
    {
        CartScanFunction(wayPoint);
    }
    catch (const std::exception&)
    {
    }
}

void CartForceScan::RefreshAdmittancePara()
{
    deltaFkminus1.setZero();
    deltaFkminus2.setZero();
    Vkminus1.setZero();
    Vkminus2.setZero();

    tool2Basekminus1 = Eigen::Matrix4d::Identity();
    first = true;

    M.setZero();
    B.setZero();
    K.setZero();
    Md.setZero();
    Bd.setZero();
    Kd.setZero();
}

/**
 * \brief 笛卡尔恒力扫查回调函数
 * \param robotState 机器人实时状态
 */
void CartForceScan::CartScanFunction(WayPoint& wayPoint)
{
    // 获取当前机器人位置
    Eigen::Matrix4d tool2BaseNow = Eigen::Map<Eigen::Matrix4d>(_curCart.position).transpose();

    if (first)
    {
        tool2Basekminus1 = tool2BaseNow;
        first = false;
    }

    // 传输六维力信息
    Sophus::Vector6d toolFk = Eigen::Map<Sophus::Vector6d>(_curForce.GenForce);

    // 获取机器人目标参考位置
    Eigen::Matrix4d tool2BaseRefer = Eigen::Map<Eigen::Matrix4d>(_refCart.position).transpose();

    //The transformation of force/torques in different coordinate systems
    Sophus::SE3d tool2BaseT(tool2BaseNow);

    Sophus::Matrix6d AdjTool2Base = tool2BaseT.Adj();

    Sophus::Vector6d baseFk = AdjTool2Base * toolFk;

    // 机器人基座坐标系中的期望力
    Sophus::Vector6d baseFd;
    baseFd << baseFk(0), baseFk(1), 10, baseFk(3), baseFk(4), baseFk(5);

    // deltaF
    Sophus::Vector6d deltaFk = baseFk - baseFd;

    // deltaF -> deltaT
    Eigen::Matrix4d deltaT = Eigen::Matrix4d::Identity();

#if false //方法一
    Sophus::Matrix6d P = Md + Bd * Ts + Kd * Ts * Ts;

    Sophus::Matrix6d alpha0 = Ts * Ts * P.inverse();
    Sophus::Matrix6d alpha1 = (2 * Md + Bd * Ts) * P.inverse();
    Sophus::Matrix6d alpha2 = -Md * P.inverse();

    Sophus::Vector6d Vk = alpha0 * deltaFk + alpha1 * Vkminus1 + alpha2 * Vkminus2;

    deltaT = Sophus::SE3d::exp(Vk - Vkminus1).matrix();
#else //方法二
    Sophus::Matrix6d Q = 4 * Md + 2 * Bd * Ts + Kd * Ts * Ts;

    Sophus::Matrix6d beta0 = Ts * Ts * Q.inverse();
    Sophus::Matrix6d beta1 = 2 * Ts * Ts * Q.inverse();
    Sophus::Matrix6d beta2 = -(2 * Kd * Ts * Ts - 8 * Md) * Q.inverse();
    Sophus::Matrix6d beta3 = -(4 * Md - 2 * Bd * Ts + Kd * Ts * Ts) * Q.inverse();

    Sophus::Vector6d Vk = beta0 * deltaFk + beta1 * deltaFkminus1 + beta0 * deltaFkminus2 + beta2 * Vkminus1 + beta3 * Vkminus2;

    deltaT = Sophus::SE3d::exp(Vk - Vkminus1).matrix();
#endif //理论上方法二更好，实际上由于RCI内部可能存在的调节机制两种方法效果差不多

    deltaFkminus2 = deltaFkminus1;
    deltaFkminus1 = deltaFk;
    Vkminus2 = Vkminus1;

    //将真实位移加入算法过程
    if (false)
    {
        Eigen::Matrix4d deltaTkminus1 = tool2BaseNow * tool2Basekminus1.inverse();
        Sophus::SE3d tmp(deltaTkminus1);
        Vkminus1 = tmp.log() + Vkminus2;
    }
    else
    {
        Vkminus1 = Vk;
    }

    tool2Basekminus1 = tool2BaseNow;

    // X = XRefer + XDelta
    Eigen::Matrix4d tool2BaseTarget = deltaT * tool2BaseRefer;
    //Eigen::Matrix4d tool2BaseTarget = tool2BaseRefer;

    Eigen::Map<Eigen::Matrix<double, 4, 4, Eigen::RowMajor>>(wayPoint.cartPos.position) = tool2BaseTarget;

#if true

    //欧拉角是多少
    Eigen::Vector3d eulerNow = tool2BaseNow.block<3, 3>(0, 0).eulerAngles(2, 1, 0);
    Eigen::Vector3d eulerDelta = deltaT.block<3, 3>(0, 0).eulerAngles(2, 1, 0);
    Eigen::Vector3d eulerTarget = tool2BaseTarget.block<3, 3>(0, 0).eulerAngles(2, 1, 0);

    LogInfo((
        std::to_string(toolFk(0)) + " " +
        std::to_string(toolFk(1)) + " " +
        std::to_string(toolFk(2)) + " " +
        //std::to_string(toolFk(3))  + " " +
        //std::to_string(toolFk(4))  + " " +
        //std::to_string(toolFk(5))  + " " +
        std::to_string(baseFk(0)) + " " +
        std::to_string(baseFk(1)) + " " +
        std::to_string(baseFk(2)) + " " +
        //std::to_string(baseFk(3))  + " " +
        //std::to_string(baseFk(4))  + " " +
        //std::to_string(baseFk(5))  + " " +
        std::to_string(tool2BaseNow(0, 3)) + " " +
        std::to_string(tool2BaseNow(1, 3)) + " " +
        std::to_string(tool2BaseNow(2, 3)) + " " +
        std::to_string(tool2BaseRefer(0, 3)) + " " +
        std::to_string(tool2BaseRefer(1, 3)) + " " +
        std::to_string(tool2BaseRefer(2, 3)) + " " +
        //std::to_string(eulerNow(0))  + " " +
        //std::to_string(eulerNow(1))  + " " +
        //std::to_string(eulerNow(2))  + " " +
        std::to_string(tool2BaseTarget(0, 3)) + " " +
        std::to_string(tool2BaseTarget(1, 3)) + " " +
        std::to_string(tool2BaseTarget(2, 3)) + " " +
        //std::to_string(eulerTarget(0))  + " " +
        //std::to_string(eulerTarget(1))  + " " +
        //std::to_string(eulerTarget(2))  + " " +
        std::to_string(deltaT(0, 3)) + " " +
        std::to_string(deltaT(1, 3)) + " " +
        std::to_string(deltaT(2, 3)) + " " +
        //std::to_string(eulerDelta(0))  + " " +
        //std::to_string(eulerDelta(1))  + " " +
        //std::to_string(eulerDelta(2))  + " " +
        "\n").c_str());

#endif
}