#include <iostream>
#include <fstream>
#include <string>
#include <string.h>
#include <cmath>
#include <stdlib.h>
#include "rotation.h"
#include "include/1mathMyDefine.h"
#include "include/initial.h"
#include "include/class.h"
#include "include/file.h"
#include "include/inertial_navigation.h"
#include "include/GINS.h"
#include <ctime>
#include <Python.h>

using namespace std;
using namespace Eigen;

int flag; // flag 用于确定中断时间

int looseCouple(
        string fileGNSS,
        string fileName,
        string fileOut,
        string filename_out_imu_error_bin,
        string filename_out_p,
        int switch_off[],
        int if_gnss_inter
) {
    /*设置cout输出位数*/
    cout.precision(20);//控制输出位数，如果太小的话，输出没有小数
    /////设置矩阵的维数，以及设置矩阵到底不需要曾广那些误差状态量
    swit(switch_off, n);

    /*gnss file*/
    bool if_end = 0;
    ifstream inFileGNSS(fileGNSS, ios::in);
    if (!inFileGNSS) {
        cout << "Can't open the GNSS file!" << endl;
        if_end = 1;
    }
    else {
        cout << "Successfully open the GNSS file!" << endl;
    }
    /*imu file*/
    ifstream inFile(fileName, ios::in);
    if (!inFile) {
        cout << "Can't open the imu file!" << endl;
        if_end = 1;
    } else {
        cout << "Successfully open the imu file!" << endl;
    }
    if (check_T_imu(inFile) == false){//检查IMU配置的周期是否正确，如果错误，直接结束程序
        return 0;
    }
    inFile.close();//关闭文件，因为已经读了两行
    inFile.open(fileName, ios::in);//为了避免数据的丢失，重新打开
    /*out result file*/
    ofstream outFile(fileOut, ios::out);
    if (!outFile) {
        cout << "Can't open the result file!" << endl;
        if_end = 1;
    } else {
        cout << "Successfully open the result file!" << endl;
    }
    /*out imu correct file*/
    // ofstream file_out_imu_correct(filename_out_imu_correct, std::ios::out);
    // if (!file_out_imu_correct){
    //     std::cout << "Can't open the corrected_imu.txt file!" << std::endl;
    //     if_end = 1;
    // }
    // else {
    //     std::cout << "Successfully open the corrected_imu.txt file!" << std::endl;
    // }
    /*out correct bin file*/
    // ofstream file_out_imu_correct_bin(filename_out_imu_correct_bin);
    // if (!file_out_imu_correct_bin){
    //     std::cout << "Can't open the corrected_imu.bin file!" << std::endl;
    //     if_end = 1;
    // }
    // else {
    //     std::cout << "Successfully open the corrected_imu.bin file!" << std::endl;
    // }
    /*out imu error file*/
    ofstream file_out_imu_error_bin(filename_out_imu_error_bin);
    if (!file_out_imu_error_bin){
        std::cout << "Can't open the imu_error file!" << std::endl;
        if_end = 1;
    }
    else {
        std::cout << "Successfully open the imu_error file!" << std::endl;
    }
    /*out p file*/
    ofstream file_out_p(filename_out_p, std::ios::out);
    if (!file_out_p){
        std::cout << "Can't open the p file!" << std::endl;
        if_end = 1;
    }
    else {
        std::cout << "Successfully open the p file!" << std::endl;
    }
    if (if_end == 1){
        return 0;//如果有一个文件打不开，程序直接终止
    }




    if (if_gnss_inter == 1){
        string fileGNSS1 = "/home/wangguan/Desktop/GNSS_RTK1.txt";//测试中断
        ofstream out1(fileGNSS1, ios::out);
        string gnss;
        int count = 0;
        while (getline(inFileGNSS, gnss)){
            count++;

            if (count == flag){
                for (int i =0; i < inter_length; i++){
                    getline(inFileGNSS, gnss);
                    count++;
                }
                flag += inter_circle;
            }

            out1 << gnss;
            out1 << endl;
        }
        inFileGNSS.close();
        out1.close();
        inFileGNSS.open("/home/wangguan/Desktop/GNSS_RTK1.txt", ios::in);
        if (!inFileGNSS) {
            cout << "Can't open the file!" << endl;
        } else {
            cout << "Successfully open the file!" << endl;
        }
    }





    ////初始化dadta
    /*get imuDataBefore from bin 文件 也就是 read the first imuData*/
    IMUDATA imuDataNow;
    IMUDATA imuDataBefore;//前一时刻imudata
    double imuDataALine[7];
    int times = 1;
    while(abs(imuDataBefore.getTime() - init_time) > T_imu / 2.0){//两个文件的开始时刻不一样，通过循环保持一致
        inFile.read((char *)imuDataALine, 56);
        imuDataBefore = getImuData(imuDataALine, 1);//0表示，读取的数据是以deg为单位,其他或者默认数据都是以rad为单位
        if ((imuDataBefore.getTime() > init_time) && (times == 1)){//如果是第一次，就要判断一下是否imu的时间已经超过了init_time
            std::cout << "最开始的imu是" << imuDataBefore.getTime() << std::endl;
            std::cout << "初始化时刻是" << init_time << std::endl;
            std::cout << "imu起始时间大于初始化时间，请检查初始化是否正确" << std::endl;
            return 0;
        }
        if (times == 1){
            times++;
        }
    }

    /*当前时刻的 state*/
    State stateNow;
    /*前一个时刻的状态*/
    State stateBefore;
    initialStateBefore(stateBefore, imuDataBefore);
    /*前两个时刻的状态*/
    //get stateBeforeBefore,因为惯性导航需要用到前两个时刻的状态，但是初始时刻前两个时刻的状态没有，
    //因此我们用k-1时刻的状态赋值给k-2时刻的状态，只是时间会变化而已
    State stateBeforeBefore;
    stateBeforeBefore = State(stateBefore.get_att(),
                              stateBefore.get_vel(),
                              stateBefore.get_pos(),
                              stateBefore.getTime() - T_imu);
    /*gnssData*/
    GNSSDATA gnssData;
    string gnssDataALine;





    /////初始化组合导航需要用到的状态量及其矩阵
    //delta_kBefore，误差状态向量
    Vector<double, Dynamic> delta_kBefore;
    //error_gyro_accel，imu的误差
    Vector<double, 24> error_gyro_accel;

    //p_kBefore，p阵，需要初始化的一个矩阵之一
    Matrix<double, Dynamic, Dynamic> p_kBefore;
    //q阵，后续需要使用
    Matrix<double, Dynamic, Dynamic> q;
    initial_Matrix(delta_kBefore,
                   error_gyro_accel,
                   p_kBefore,
                   q);




    ////正式进行数据更新
    while (inFile.read((char *)imuDataALine, 56)){//从bin文件读取的时候，判断如何结束
        /*得到现在时刻的imu数据*/
        imuDataNow = getImuData(imuDataALine, 1);//0代表数据以deg为单位，1或者其他的数字代表是rad

        ////校正陀螺仪和加速度计
        correctGyroAccel(error_gyro_accel,
                         imuDataNow,
                         imuDataBefore);


        ////out the data
        // out_cor_imu(imuDataNow, file_out_imu_correct);
        // out_cor_imu_bin(imuDataNow, file_out_imu_correct_bin);
        out_imu_error(error_gyro_accel, imuDataNow.getTime(), file_out_imu_error_bin);
        out_p(p_kBefore, imuDataBefore, file_out_p);



        ////机械编排
        stateNow = inertial_navigation(stateBeforeBefore,
                                     stateBefore,
                                     stateBefore.get_att_quat(),
                                     imuDataBefore,
                                     imuDataNow);


        ////预测
        Matrix<double, Dynamic, Dynamic> p_k_kBefore;
        Vector<double, Dynamic> delta_k_kBefore;
        pred(p_k_kBefore,
             delta_k_kBefore,
             delta_kBefore,
             p_kBefore,
             q,
             stateBefore,
             imuDataBefore,
             stateNow,
             imuDataNow);

        ////如果gnsstime要小于当前的时间，无法使用，必须更新gnss数据,而且还要判断gnss数据是否读完
        bool gnssDataNotOver = true;
        while(gnssDataNotOver && (gnssData.getTime() - stateNow.getTime() < -0.0001)){//gnss数据在ins数据之前，就要先更新gnss数据
            if (!getline(inFileGNSS, gnssDataALine)) {
                gnssDataNotOver = false;
            }
            if (gnssDataALine.length() > 10){
                gnssData = getGNSSDATA(gnssDataALine);
            }
        }

        Matrix<double, Dynamic, Dynamic> p_k;//p_k;
        Vector<double, Dynamic> delta_k;//delta_k;
        ////如果和当前gnsstime差距很小，不外推
        if (abs(gnssData.getTime() - stateNow.getTime()) <= 0.0001){
            ////直接进行组合导航推算（量测更新）
            MeasUpdate(p_k,
                       delta_k,
                       p_k_kBefore,
                       delta_k_kBefore,
                       stateNow,
                       gnssData);

            ////对imu输出数据进行校正
            correctOut(stateNow,
                       delta_k);

            ////get error_gyro_accel
            getErrorGA(delta_k,
                       error_gyro_accel);

            /////更新下一次预测需要用到的初始值
            /*更新p_k_1*/
            p_kBefore = p_k;
            /*更新delta_kBefore*/
            delta_kBefore = delta_k;
        }

        ////如果gnssdata.time()位于这一例元和下一例元之间
        else if (((gnssData.getTime() - stateNow.getTime()) > 0.0001)&&
                 ((gnssData.getTime() - stateNow.getTime()) < T_imu - 0.0001)){

            if (inFile.read((char *)imuDataALine, 56)){//如果还有下一时刻数据
                /////更新下一次预测需要用到的初始值,由于没经过量测方程，因此，直接用预测值变成下一次的
                /*更新p_k_1*/
                p_kBefore = p_k_kBefore;
                /*更新delta_kBefore*/
                delta_kBefore = delta_k_kBefore;

                ////后续组合导航数据更新处理
                /*更新前两个例元的状态和四元数*/
                stateBeforeBefore = stateBefore;
                /*更新前一个例元的状态和四元数*/
                stateBefore = stateNow;
                /*更新当前一时刻得到imudata*/
                imuDataBefore = imuDataNow;
                outTextFile(outFile, stateNow);

                ////对下一时刻进行预测
                /*get下一时刻的数据*/
                IMUDATA imuDataNext = getImuData(imuDataALine, 1);//0代表数据以deg,1 is rad


                ////////切割重新得到imudataNow，切割重新得到imuDataNext
                double t = imuDataNext.getTime() - imuDataBefore.getTime();
                double t1 = gnssData.getTime() - imuDataBefore.getTime();
                double t2 = imuDataNext.getTime() - gnssData.getTime();
                Vector3d gyroNow = imuDataNext.getGyroData() * t1 / t;
                Vector3d accelNow = imuDataNext.getAccelData() * t1 / t;
                double timeNow = gnssData.getTime();
                imuDataNow = IMUDATA(gyroNow, accelNow, timeNow);
                Vector3d gyroNext = imuDataNext.getGyroData() * t2 / t;
                Vector3d accelNext = imuDataNext.getAccelData() * t2 / t;
                double timeNext = imuDataNext.getTime();
                imuDataNext = IMUDATA(gyroNext, accelNext, timeNext);

                //校正数据imuDataNow,切割成两个部分后，由于imu误差更新，所以说，要分成两个部分校正
                correctGyroAccel(error_gyro_accel,
                                 imuDataNow,
                                 imuDataBefore);

                ////机械编排
                stateNow = inertial_navigation(stateBeforeBefore,
                                              stateBefore,
                                              stateBefore.get_att_quat(),
                                              imuDataBefore,
                                              imuDataNow);

                ////组合导航预测更新
                pred(p_k_kBefore,
                     delta_k_kBefore,
                     delta_kBefore,
                     p_kBefore,
                     q,
                     stateBefore,
                     imuDataBefore,
                     stateNow,
                     imuDataNow);

                ////直接进行组合导航推算（量测更新）
                MeasUpdate(p_k,
                           delta_k,
                           p_k_kBefore,
                           delta_k_kBefore,
                           stateNow,
                           gnssData);

                ////对机械编排结果进行校正
                correctOut(stateNow,
                           delta_k);

                ////get error_gyro_accel
                getErrorGA(delta_k,
                           error_gyro_accel);


                ////重新对下一时刻进行量测更新,为了方便使用之前的代码，我们需要变换时23刻
                //将下一时刻视作当前时刻
                //将当前时刻视作前一时刻
                //将前一时刻视作前两个时刻

                /////更新下一次预测需要用到的初始值
                /*更新p_k_1*/
                p_kBefore = p_k;
                /*更新delta_kBefore*/
                delta_kBefore = delta_k;

                ////更新下一次预测需要用到的imudata
                imuDataBefore = imuDataNow;
                imuDataNow  = imuDataNext;
                /*更新前两个例元的状态*/
                stateBeforeBefore = stateBefore;
                /*更新前一时刻四元数*/
                stateBefore = stateNow;

                ////校正陀螺仪和加速度计
                correctGyroAccel(error_gyro_accel,
                                 imuDataNow,
                                 imuDataBefore);

                ////机械编排
                stateNow = inertial_navigation(stateBeforeBefore,
                                                stateBefore,
                                                stateBefore.get_att_quat(),
                                                imuDataBefore,
                                                imuDataNow);

                ////预测
                Matrix<double, Dynamic, Dynamic> p_k_kBefore;
                Vector<double, Dynamic> delta_k_kBefore;
                pred(p_k_kBefore,
                     delta_k_kBefore,
                     delta_kBefore,
                     p_kBefore,
                     q,
                     stateBefore,
                     imuDataBefore,
                     stateNow,
                     imuDataNow);

                ////更新p_k_1和delta_kBefore用于下一时刻的解算
                /*更新p_k_1*/
                p_kBefore = p_k_kBefore;
                /*更新delta_kBefore*/
                delta_kBefore = delta_k_kBefore;//加上之前被消去的0偏，才是最后真正的delta_k,才能用与下一个例元解算
            }
        }

        ////如果接近了下一例元，或者超出了下一例元直接不进行量测更新
        else {
            /////更新下一次预测需要用到的初始值,由于没经过量测方程，因此，直接用预测值变成下一次的
            /*更新p_k_1*/
            p_kBefore = p_k_kBefore;
            /*更新delta_kBefore*/
            delta_kBefore = delta_k_kBefore;//加上之前被消去的0偏，才是最后真正的delta_k,才能用与下一个例元解算
        }

        ////后续组合导航数据更新处理
        /*更新前两个例元的状态和四元数*/
        stateBeforeBefore = stateBefore;
        /*更新前一个例元的状态和四元数*/
        stateBefore = stateNow;
        /*更新当前一时刻得到imudata*/
        imuDataBefore = imuDataNow;

        ////output data
        outTextFile(outFile, stateNow);
    }
    inFileGNSS.close();
    outFile.close();
    inFile.close();
    // file_out_imu_correct.close();
    // file_out_imu_correct_bin.close();
    file_out_imu_error_bin.close();
    file_out_p.close();

    return 0;
}

int PlotIMUError(std::string imu_error_file) {
    // 1.初始化 Python接口
    Py_Initialize();

    // 2. 初始化使用的变量
    PyObject* pModule = NULL;
    PyObject* pFunc = NULL;
    PyObject* pName = NULL;

    // 3、初始化python系统文件路径，保证可以访问到 .py文件
    PyRun_SimpleString("import sys");
    // 最好要带上字符 r，防止转移
    // 一定要使用绝对路径
    PyRun_SimpleString("sys.path.append(r'/home/wangguan/Desktop/loose_couple_all_wg_v2.0/script')");
    PyRun_SimpleString("print(sys.path)");

    // 4.调用Python文件名字和函数名字
    pModule = PyImport_ImportModule("plot_imu_error"); // plot_imu_error.py
    if( pModule == NULL ) {
        cout << "module not found" << endl;
        return -1;
    }
    pFunc = PyObject_GetAttrString(pModule, "plot_error"); // plot_error function
    if( !pFunc || !PyCallable_Check(pFunc)){
        cout <<"not found function add_num" << endl;
        return -1;
    }

    // 5.传入参数
    // string 2 char *
    int char_length = std::strlen(imu_error_file.c_str()) + 1;
    char* py_name = new char[char_length];
    std::strcpy(py_name,  imu_error_file.c_str());
    PyObject* pArgs = PyTuple_New(1);
    // 传入参数
    PyTuple_SetItem(pArgs, 0, Py_BuildValue("s", py_name));

    // 6.运行
    PyEval_CallObject(pFunc, pArgs);

    // 7.结束python接口初始化
    Py_Finalize();
}

int PlotP(std::string imu_error_file) {
    // 1.初始化 Python接口
    Py_Initialize();

    // 2. 初始化使用的变量
    PyObject* pModule = NULL;
    PyObject* pFunc = NULL;
    PyObject* pName = NULL;

    // 3、初始化python系统文件路径，保证可以访问到 .py文件
    PyRun_SimpleString("import sys");
    // 最好要带上字符 r，防止转移
    // 一定要使用绝对路径
    PyRun_SimpleString("sys.path.append(r'/home/wangguan/Desktop/loose_couple_all_wg_v2.0/script')");
    PyRun_SimpleString("print(sys.path)");

    // 4.调用Python文件名字和函数名字
    pModule = PyImport_ImportModule("plot_p"); // plot_p.py
    if( pModule == NULL ) {
        cout << "module not found" << endl;
        return -1;
    }
    pFunc = PyObject_GetAttrString(pModule, "plot_p"); // plot_error function
    if( !pFunc || !PyCallable_Check(pFunc)){
        cout <<"not found function add_num" << endl;
        return -1;
    }

    // 5.传入参数
    // string 2 char *
    int char_length = std::strlen(imu_error_file.c_str()) + 1;
    char* py_name = new char[char_length];
    std::strcpy(py_name,  imu_error_file.c_str());
    PyObject* pArgs = PyTuple_New(1);
    // 传入参数
    PyTuple_SetItem(pArgs, 0, Py_BuildValue("s", py_name));
//    PyObject* pArgs = PyTuple_New(3);
//    // 传入参数
//    PyTuple_SetItem(pArgs, 0, Py_BuildValue("s", py_name));
//    PyTuple_SetItem(pArgs, 1, Py_BuildValue("i", gyro_mode));
//    PyTuple_SetItem(pArgs, 2, Py_BuildValue("i", accel_mode));

    // 6.运行
    PyEval_CallObject(pFunc, pArgs);

    // 7.结束python接口初始化
    Py_Finalize();
}

template<class T>
std::string toString(const T& t){
    std::ostringstream os;
    os << t;
    return os.str();
}

int main(int argc, char* argv[]) {
    if (argc != 2) {
        std::cout << "Please input the yaml file!" << std::endl;
        return -1;
    }

    // load config
    std::string config_file = argv[1];
    if (!LoadConfig(config_file)) {
        return -1;
    }

    if (gnss_inter) {
        // loose couple
        for (int i = 0; i < sizeof(inter_start_time) / sizeof(inter_start_time[0]); i++){
            flag = inter_start_time[i];
            string start_time = toString(flag);
            string prefix = GetSaveFilePrefix(start_time);
            string filename_out_result = save_path + "/naverr/" + prefix + ".nav";
            string filename_out_imu_error = save_path + "/imuerr/" + prefix + "_imu_err.bin";
            string filename_out_p = save_path + "/p/" + prefix + "_p.bin";
            // string filename_out_imu_cor_bin = prefix + "_imu_cor.bin";
            // string filename_out_imu_cor_asc = prefix + "_imu_cor.txt";

            looseCouple(gnss_file,
                        imu_file,
                        filename_out_result,
                        filename_out_imu_error,
                        filename_out_p,
                        switch_off,
                        gnss_inter);

            // use python to plot
            // int gyro_mode = gyro_non_mode;
            // int accel_mode = accel_non_mode;
            // PlotIMUError(filename_out_imu_error, gyro_mode, accel_mode);
        }
    }
    else {
        flag = inter_start_time[0];
        string start_time = toString(flag);
        string prefix = GetSaveFilePrefix(start_time);
        string filename_out_result = save_path + "/naverr/" + prefix + ".nav";
        string filename_out_imu_error = save_path + "/imuerr/" + prefix + "_imu_err.bin";
        string filename_out_p = save_path + "/p/" + prefix + "_p.bin";
        // string filename_out_imu_cor_bin = prefix + "_imu_cor.bin";
        // string filename_out_imu_cor_asc = prefix + "_imu_cor.txt";

        looseCouple(gnss_file,
                    imu_file,
                    filename_out_result,
                    filename_out_imu_error,
                    filename_out_p,
                    switch_off,
                    gnss_inter);

        // use python to plot
         int gyro_mode = gyro_non_mode;
         int accel_mode = accel_non_mode;
        // PlotIMUError(filename_out_imu_error);
        // PlotP(filename_out_p);
    }


}