#include "CartFreeDrag.h"

void CartFreeDrag::Init()
{
    isDrag = false;
    // 导纳控制参数 K=0
    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() << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0;

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

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

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

    Y = 0;
    N = 0;

    // 启动力阈值
    //dragFThr << 1.0, 2.0, 2.0, 0.2, 0.2, 0.1; // RoundProde
    dragFThr << 1.0, 2.0, 3.0, 0.3, 0.1, 0.1; // S16Pv
}

void CartFreeDrag::LoadData(WayPoint curWayPt, FTData curForce, WayPoint refWayPt)
{
    //拖动模式下只需要当前的笛卡尔力和当前的笛卡尔位姿
    _curCart = curWayPt.cartPos;
    _curForce = curForce.CartFT;
}

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

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

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

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

    dragFThr.setZero();
    isDrag = false;
    Y = 0;
    N = 0;
}

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

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

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

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

    Sophus::Vector6d baseFk = AdjTool2Base * toolFk;

    // 期望力为0
    // deltaF
    Sophus::Vector6d deltaFk = baseFk;

    // 拖了吗
    if (!isDrag)
    {
        for (size_t i = 0; i < 6; i++)
        {
            if (abs(toolFk(i)) >= dragFThr(i))
            {
                Y++;
                break;
            }
            else
            {
                Y = 0;
            }
        }
        if (Y >= 50)
        {
            isDrag = true;
            Y = 0;
            N = 0;
            deltaFkminus1.setZero();
            deltaFkminus2.setZero();
            Vkminus1.setZero();
            Vkminus2.setZero();

            tool2Basekminus1 = tool2BaseNow;
        }
    }
    else
    {
        for (size_t i = 0; i < 6; i++)
        {
            if (abs(toolFk(i)) < dragFThr(i))
            {
                N++;
            }
            else
            {
                N = 0;
            }
            if (N >= 50)
            {
                isDrag = false;
                N = 0;
                Y = 0;
                deltaFkminus1.setZero();
                deltaFkminus2.setZero();
                Vkminus1.setZero();
                Vkminus2.setZero();

                tool2Basekminus1 = tool2BaseNow;
            }
        }
    }

    // deltaF -> deltaT
    Eigen::Matrix4d deltaT = Eigen::Matrix4d::Identity(4, 4);
    if (isDrag)
    {
#if false //方法一
        Sophus::Matrix6d P = 2 * Md + Bd * Ts;
        Sophus::Matrix6d Q = 2 * Md - Bd * Ts;

        Sophus::Vector6d Vk = P.inverse() * ((deltaFk + deltaFkminus1) * Ts + Q * Vkminus1);

        deltaT = Sophus::SE3d::exp((Vk + Vkminus1) / 2 * Ts).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 //两种方法结果一样

        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 * tool2BaseNow;

    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(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))  + " " +
        std::to_string((int)isDrag) + "\n").c_str());

#endif
}