#include <pose_graph/pose_graph.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 TranslationLocalParameterization::Plus(const double *x, const double *delta, double *x_plus_delta) const
{
    Eigen::Map<const Eigen::Vector3d> _p(x);

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

    Eigen::Map<Eigen::Vector3d> p(x_plus_delta);

    p = _p + dp;

    return true;
}

bool TranslationLocalParameterization::ComputeJacobian(const double *x, double *jacobian) const
{
    Eigen::Map<Eigen::Matrix<double, 3, 3, Eigen::RowMajor>> j(jacobian);
    j.setIdentity();

    return true;
}

bool QuaternionLocalParameterization::Plus(const double *x, const double *delta, double *x_plus_delta) const
{
    Eigen::Map<const Eigen::Quaterniond> _q(x);

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

    Eigen::Map<Eigen::Quaterniond> q(x_plus_delta);

    q = (_q * dq).normalized();
    return true;
}

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

    return true;
}

PoseGraphFunction::PoseGraphFunction() 
{}

bool PoseGraphFunction::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[1][3], parameters[1][0], parameters[1][1], parameters[1][2]);
    
    Eigen::Vector3d T2(parameters[2][0], parameters[2][1], parameters[2][2]);
    Eigen::Quaterniond Q2(parameters[3][3], parameters[3][0], parameters[3][1], parameters[3][2]);
    
    Eigen::Vector3d T12(parameters[4][0], parameters[4][1], parameters[4][2]);
    Eigen::Quaterniond Q12(parameters[5][3], parameters[5][0], parameters[5][1], parameters[5][2]);
    
    Eigen::Map<Eigen::Vector3d> residual(residuals);
    
    Eigen::Matrix3d R1 = Q1.toRotationMatrix();
    Eigen::Matrix3d R2 = Q2.toRotationMatrix();
    Eigen::Matrix3d R12 = Q12.toRotationMatrix();

    residual = -R12 * R1.inverse() * T1 + R2 * T12 + T2;

    if (jacobians) {
        // 对T1求导
        if (jacobians[0]) {
            Eigen::Map<Eigen::Matrix<double, 3, 3, Eigen::RowMajor>> jacobian(jacobians[0]);

            jacobian = -R12 * R1.inverse();
        }
        // 对R1求导
        if (jacobians[1]) {
            Eigen::Map<Eigen::Matrix<double, 3, 4, Eigen::RowMajor>> jacobian(jacobians[1]);
            Eigen::Matrix<double, 3, 4> jaco;

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

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

            jacobian = R2;
        }
        // 对R12求导
        if (jacobians[5]) {
            Eigen::Map<Eigen::Matrix<double, 3, 4, Eigen::RowMajor>> jacobian(jacobians[5]);
            Eigen::Matrix<double, 3, 4> jaco;

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

}
}