#include "imucaliber.h"

namespace juefx{

ImuCaliber::Ptr imuCaliberInstance;

ImuCaliber::ImuCaliber()
{

}

void ImuCaliber::Reset()
{
    imuCaliberInstance.reset();
}

ImuCaliber::Ptr ImuCaliber::Instance()
{
    if(!imuCaliberInstance){
        imuCaliberInstance.reset(new ImuCaliber());
    }
    return imuCaliberInstance;
}

bool ImuCaliber::CalculateImuPose(const IMUPacket &imu)
{
    auto imu2base = Parameter::Instance()->GetImu2Base();
    const auto &imu_acc = imu2base.linear().cast<float>() * imu.linear_acceleration;
    if(dataCnt_ == 0){
        meanAcc_ = imu_acc;
    }else{
        meanAcc_ += (imu_acc - meanAcc_) / dataCnt_;
        if(dataCnt_ >= 100){
            Vector3f grav = Vector3f(0, 0, -meanAcc_.norm());
            Vector3f body_gravity_vector = meanAcc_;
            Vector3f map_gravity_vector = Vector3f::UnitZ();
            Quaternionf rotation = Quaternionf::FromTwoVectors(body_gravity_vector, map_gravity_vector);
            rotation = rotation.normalized();
            Isometry3f eskf_init_pose = Isometry3f::Identity();
            eskf_init_pose.linear() = rotation.toRotationMatrix();
            Parameter::Instance()->SetEskfInitPose(eskf_init_pose);
            Parameter::Instance()->SetGravity(grav);
            auto rpy = RotationQuaternionToEulerVector(
                        Quaternionf(eskf_init_pose.linear()));
            std::ofstream ofs(Parameter::Instance()->GetBaseDir() + "/eskf_init_pose.txt", ios::out);
            ofs << eskf_init_pose.translation().x() << ", " << eskf_init_pose.translation().y() <<  ", "
                << eskf_init_pose.translation().x() << ", " << rpy.x() <<  ", "
                << rpy.y() << ", " << rpy.z();
            ofs.close();
            return true;
        }
    }
    dataCnt_++;
    return false;
}

} // end of namespace