#include "align.h"

namespace uwb_slam{
    void Align::Run()
    {
        imuDataRxTime = ros::Time::now();
        uwbDataRxTime = ros::Time::now();

        wheel_odom_sub_= nh_.subscribe("wheel_odom",10,&Align::wheel_odomCB,this);
        imu_sub_= nh_.subscribe("raw_imu",10,&Align::imuCB,this);
        odom_sub_= nh_.subscribe("odom",10,&Align::odomCB,this);

        std::ofstream outfile("data.txt",std::ofstream::out);
        if(outfile.is_open()) {
            std::cout << "start saving data" << std::endl;
            outfile << "imuDataRxTime,odomDataRxTime,uwbDataRxTime,"\
                    << "aX,aY,aZ,"\
                    << "gX,gY,gZ"\
                    << "mX,mY,mZ,"\
                    << "qW,qX,qY,qZ,"\
                    << "vXY,angleV,"\
                    << "imuPosX,imuPosY,"\
                    << "lightHousePosX,lightHousePosY,lightHousePosZ,"\
                    << "lightHouseQW,lightHouseQX,lightHouseQY,lightHouseQZ"\
                    << "d1,d2,d3\n";
        } else {
            std::cout<<"file can not open"<<std::endl;
        }

        while(1){
            if(imuDataRxTime!=imu_odom_.imu_data_.imu_t_){
                //std::cout << "imu received" << std::endl;
                imuDataRxTime = imu_odom_.imu_data_.imu_t_;
                odomDataRxTime = odom_tmp_;
                uwbDataRxTime = uwb_->uwb_data_.uwb_t_;
    
                outfile << imuDataRxTime << "," << odomDataRxTime << "," << uwbDataRxTime <<","\
                        << imu_odom_.imu_data_.a_[0] << "," << imu_odom_.imu_data_.a_[1] << "," << imu_odom_.imu_data_.a_[2] << ","\
                        << imu_odom_.imu_data_.g_[0] << "," << imu_odom_.imu_data_.g_[1] << "," << imu_odom_.imu_data_.g_[2] << ","\
                        << imu_odom_.imu_data_.m_[0] << "," << imu_odom_.imu_data_.m_[1] << "," << imu_odom_.imu_data_.m_[2] << ","\
                        << imu_odom_.quat_[0] << "," << imu_odom_.quat_[0] << "," << imu_odom_.quat_[0] << "," << imu_odom_.quat_[0] << ","\
                        << imu_odom_.vxy_ << "," << imu_odom_.angle_v_ << ","\
                        << imu_odom_.pose_[0] << "," << imu_odom_.pose_[1] << ","\
                        << lighthouse_->data.x_ << "," << lighthouse_->data.y_ << "," << lighthouse_->data.z_ << ","\
                        << lighthouse_->data.qw_ << "," << lighthouse_->data.qx_ << "," << lighthouse_->data.qy_ << "," << lighthouse_->data.qz_ << ","\
                        << uwb_->d[0] << "," << uwb_->d[1] << "," << uwb_->d[2] << "\n";
            }
        }
        // outfile.close();
        // std::cout<< "Data written to file." << std::endl;   
    }
    
    
    
    void Align::wheel_odomCB(const nav_msgs::Odometry& wheel_odom)
    {
        imu_odom_.vxy_= wheel_odom.twist.twist.linear.x;
        imu_odom_.angle_v_ = wheel_odom.twist.twist.angular.z;
        return;
    }
    void Align::imuCB(const pibot_msgs::RawImu& imu)
    {
        imu_odom_.imu_data_.imu_t_ = imu.header.stamp;
        imu_odom_.imu_data_.a_[0] = imu.raw_linear_acceleration.x;
        imu_odom_.imu_data_.a_[1] = imu.raw_linear_acceleration.y;
        imu_odom_.imu_data_.a_[2] = imu.raw_linear_acceleration.z;

        imu_odom_.imu_data_.g_[0] = imu.raw_angular_velocity.x;
        imu_odom_.imu_data_.g_[1] = imu.raw_angular_velocity.y;
        imu_odom_.imu_data_.g_[2] = imu.raw_angular_velocity.z;

        imu_odom_.imu_data_.m_[0] = imu.raw_magnetic_field.x;
        imu_odom_.imu_data_.m_[1] = imu.raw_magnetic_field.y;
        imu_odom_.imu_data_.m_[2] = imu.raw_magnetic_field.z;

        return;
    }

    void Align::odomCB(const nav_msgs::Odometry& odom)
    {
        odom_tmp_ = odom.header.stamp;
        imu_odom_.pose_[0] = odom.pose.pose.position.x;
        imu_odom_.pose_[1] = odom.pose.pose.position.y;
        imu_odom_.pose_[2] = odom.pose.pose.position.z;
        imu_odom_.quat_[0] = odom.pose.pose.orientation.w;
        imu_odom_.quat_[1] = odom.pose.pose.orientation.x;
        imu_odom_.quat_[2] = odom.pose.pose.orientation.y;
        imu_odom_.quat_[3] = odom.pose.pose.orientation.z;
    }

};



