#include <pose_graph/pose_cost_function.h>

namespace hlzn_slam {
namespace pose_graph {

template <typename Derived>
static Eigen::Matrix<typename Derived::Scalar, 3, 3> skewSymmetric(const Eigen::MatrixBase<Derived> &q)
{
    Eigen::Matrix<typename Derived::Scalar, 3, 3> ans;
    ans << typename Derived::Scalar(0), -q(2), q(1),
        q(2), typename Derived::Scalar(0), -q(0),
        -q(1), q(0), typename Derived::Scalar(0);
    return ans;
}

template <typename Derived>
static Eigen::Quaternion<typename Derived::Scalar> deltaQ(const Eigen::MatrixBase<Derived> &theta)
{
    typedef typename Derived::Scalar Scalar_t;

    Eigen::Quaternion<Scalar_t> dq;
    Eigen::Matrix<Scalar_t, 3, 1> half_theta = theta;
    half_theta /= static_cast<Scalar_t>(2.0);
    dq.w() = static_cast<Scalar_t>(1.0);
    dq.x() = 0; // half_theta.x();
    dq.y() = 0; // half_theta.y();
    dq.z() = half_theta.z();
    return dq;
}

bool PoseLocalParameterization::Plus(const double *x, const double *delta, double *x_plus_delta) const
{
    Eigen::Map<const Eigen::Vector3d> _p(x);
    Eigen::Map<const Eigen::Quaterniond> _q(x + 3);

    Eigen::Map<const Eigen::Vector3d> dp(delta);

    Eigen::Quaterniond dq = deltaQ(Eigen::Map<const Eigen::Vector3d>(delta + 3));

    Eigen::Map<Eigen::Vector3d> p(x_plus_delta);
    Eigen::Map<Eigen::Quaterniond> q(x_plus_delta + 3);

    p = _p + dp;
    q = (_q * dq).normalized();

    return true;
}

bool PoseLocalParameterization::ComputeJacobian(const double *x, double *jacobian) const
{
    Eigen::Map<Eigen::Matrix<double, 7, 6, Eigen::RowMajor>> j(jacobian);
    j.topRows<6>().setIdentity();
    j.bottomRows<1>().setZero();

    return true;
}

PoseCostFunction::PoseCostFunction() 
{

}

bool PoseCostFunction::Evaluate(double const *const *parameters, double *residuals, double **jacobians) const
{
    Eigen::Vector3d T1(parameters[0][0], parameters[0][1], parameters[0][2]);
    Eigen::Quaterniond Q1(parameters[0][6], parameters[0][3], parameters[0][4], parameters[0][5]);
    
    Eigen::Vector3d T2(parameters[1][0], parameters[1][1], parameters[1][2]);
    Eigen::Quaterniond Q2(parameters[1][6], parameters[1][3], parameters[1][4], parameters[1][5]);
    
    Eigen::Vector3d T12(parameters[2][0], parameters[2][1], parameters[2][2]);
    Eigen::Quaterniond Q12(parameters[2][6], parameters[2][3], parameters[2][4], parameters[2][5]);
    
    Eigen::Map<Eigen::Vector3d> residual(residuals);
    
    Eigen::Matrix3d R1 = Q1.toRotationMatrix();
    Eigen::Matrix3d R2 = Q2.toRotationMatrix();
    Eigen::Matrix3d R12 = Q12.toRotationMatrix();

    // residual = -R1 * R12 * R2.inverse() * T2 + R1 * T12 + T1;
    
    // LOG(INFO)<<"residual: \r\n"<<residual;
    // if (jacobians) {
    //     // 对R1和T1求导
    //     if (jacobians[0]) {
    //         Eigen::Map<Eigen::Matrix<double, 3, 7, Eigen::RowMajor>> jacobian(jacobians[0]);
    //         Eigen::Matrix<double, 3, 6> jaco;

    //         jaco.leftCols<3>().setIdentity();
    //         jaco.rightCols<3>() = R1 * skewSymmetric(R12 * R2.inverse() * T2) - R1 * skewSymmetric(T12);
            
    //         jacobian.leftCols<6>() = jaco;
    //         jacobian.rightCols<1>().setZero();
    //     }
    //     // 对R2和T2求导
    //     if (jacobians[1]) {
    //         Eigen::Map<Eigen::Matrix<double, 3, 7, Eigen::RowMajor>> jacobian(jacobians[1]);
    //         Eigen::Matrix<double, 3, 6> jaco;

    //         jaco.leftCols<3>() = -R1 * R12 * R2.inverse();
    //         jaco.rightCols<3>() = -R1 * R12 * skewSymmetric(R2.inverse() * T2);
            
    //         jacobian.leftCols<6>() = jaco;
    //         jacobian.rightCols<1>().setZero();
    //     }
    //     // 对R12和T12求导
    //     if (jacobians[2]) {
    //         Eigen::Map<Eigen::Matrix<double, 3, 7, Eigen::RowMajor>> jacobian(jacobians[2]);
    //         Eigen::Matrix<double, 3, 6> jaco;

    //         jaco.leftCols<3>() = R1;
    //         jaco.rightCols<3>() = R1 * R12 * skewSymmetric(R2.inverse() * T2);
            
    //         jacobian.leftCols<6>() = jaco;
    //         jacobian.rightCols<1>().setZero();
    //     }
    // }
    Eigen::Vector3d p(1, 1, 1);
    residual = R2.inverse() * R1 * R12 * p + R2.inverse() * R1 * T12 + R2.inverse() * T1 - R2.inverse() * T2 - p;
    
    // LOG(INFO)<<"residual: \r\n"<<residual;
    if (jacobians) {
        // 对R1和T1求导
        if (jacobians[0]) {
            Eigen::Map<Eigen::Matrix<double, 3, 7, Eigen::RowMajor>> jacobian(jacobians[0]);
            Eigen::Matrix<double, 3, 6> jaco;

            jaco.leftCols<3>() = R2.inverse();
            jaco.rightCols<3>() = -R2.inverse() * R1 * skewSymmetric(R12 * p) - R2.inverse() * R1 * skewSymmetric(T12);
            // R1 * skewSymmetric(R12 * R2.inverse() * T2) - R1 * skewSymmetric(T12);
            
            jacobian.leftCols<6>() = jaco;
            jacobian.rightCols<1>().setZero();
        }
        // 对R2和T2求导
        if (jacobians[1]) {
            Eigen::Map<Eigen::Matrix<double, 3, 7, Eigen::RowMajor>> jacobian(jacobians[1]);
            Eigen::Matrix<double, 3, 6> jaco;

            jaco.leftCols<3>() = -R2.inverse();
            jaco.rightCols<3>() = skewSymmetric(R2.inverse() * R1 * R12 * p) + 
                skewSymmetric(R2.inverse() * R1 * T12) + skewSymmetric(R2.inverse() * T1) - skewSymmetric(R2.inverse() * T2);
            
            jacobian.leftCols<6>() = jaco;
            jacobian.rightCols<1>().setZero();
        }
        // 对R12和T12求导
        if (jacobians[2]) {
            Eigen::Map<Eigen::Matrix<double, 3, 7, Eigen::RowMajor>> jacobian(jacobians[2]);
            Eigen::Matrix<double, 3, 6> jaco;

            jaco.leftCols<3>() = R2.inverse() * R1;
            jaco.rightCols<3>() = -R2.inverse() * R1 * R12 * skewSymmetric(p);
            
            jacobian.leftCols<6>() = jaco;
            jacobian.rightCols<1>().setZero();
        }
    }
    return true;
}

}
}