#include<iostream>
#include<algorithm>
#include<fstream>
#include<chrono>
#include <ctime>
#include <sstream>
#include "IMUReader.h"
#include<opencv2/core/core.hpp>
#include<System.h>
#include "ImuTypes.h"

using namespace std;



int main(int argc, char *argv[])
{

    if(argc < 3)
    {
        cerr << endl << "Usage: ./mono_inertial_euroc path_to_vocabulary path_to_setting" << endl;
        return 1;
    }

    ORB_SLAM3::System SLAM(argv[1],argv[2],ORB_SLAM3::System::IMU_MONOCULAR, true);

    vector<ORB_SLAM3::IMU::Point> vImuMeas;
   

    vImuMeas.clear();
    int fps = 30;
    IMUReader imuReader ("/dev/ttyUSB0");
    std::this_thread::sleep_for(std::chrono::milliseconds(1000/fps));
  
    cv::VideoCapture cap = cv::VideoCapture(0);
    cap.set(cv::CAP_PROP_FRAME_WIDTH, 640);
    cap.set(cv::CAP_PROP_FRAME_HEIGHT, 480);

    if (!cap.isOpened()) {
        std::cerr << "Error: Could not open camera." << std::endl;
        return -1;
        }
      
    while(1)
    {
        cv::Mat im;
        cap >> im;
        if(im.empty())
        {
            cerr << endl << "Error: Couldn't capture a frame"
            << endl;
            return 1;
        }
        vImuMeas.clear();
        auto currentTime = std::chrono::high_resolution_clock::now();
        auto timestamp = std::chrono::duration_cast<std::chrono::nanoseconds>(currentTime.time_since_epoch());
        double tframe = static_cast<double>(timestamp.count());

        // IMUData data;
     
    while (!imuReader.imubuffer.empty()) {
            IMUData myImuData = imuReader.imubuffer.front();
            ORB_SLAM3::IMU::Point slamImuData(myImuData.vAcc, myImuData.vGyro, myImuData.timestamp);

            // 添加到 SLAM 系统定义的向量中
            vImuMeas.push_back(slamImuData);

            imuReader.imubuffer.erase(imuReader.imubuffer.begin());
        }
        cout << "Accumulated IMU Data Size: " << vImuMeas.size() << endl;

        // 清空 imu 缓冲区

        // imuReader.imubuffer.clear();

        // SLAM.TrackMonocular(im, tframe, vImuMeas);
        // vImuMeas.clear();
    }
     
        
        
        




    // Stop all threads
    //  SLAM.Shutdown();

}