//
// Created by Wakanda_shaw on 2022/5/4.
//
#include <bitset>
#include "main_window.h"
#include "fstream"
#include <cmath>
#include   "iomanip"


using namespace std;
int64_t exclusive_or = pow(2, 37) -1 ;
void main_window::Read_PVT(QString path, QString outpath) {
    QFile file(path);
    file.open(QIODevice::ReadOnly);
    QByteArray array = file.readAll();
    ofstream outputfile(outpath.toStdString());
    if(outputfile){
        outputfile<<"Tow"<<","<<"ecef_posX"<<","<<"ecef_posY"<<","<<"ecef_posZ"<<","<<"ecef_vX"<<","<<"ecef_vY"<<","<<"ecef_vZ"<<endl;
    }

    // 将数据类型转换为string
    string arr = array.toHex().toStdString();
    // 设定分隔符
    string split = "d300";
    // 从头开始
    int start_pos = arr.find(split, 0);
    int end_pos = arr.find(split, start_pos + 4);
    //子字符串个数
    int delta = end_pos - start_pos;
    // 截取以d300开头的子字符串
    string sub = arr.substr(start_pos, delta);

    while (true) {
        try {
            if (sub.substr(6, 5) == "3e704") {
                uint32_t epoch = uint32_t (stoul(sub.substr(40, 8), nullptr, 16)) >>1;
                epoch = epoch/1000 + 18; // ms to s and + 闰秒
                cout<<epoch<<endl;
                bitset<44> bit_x(stoll(sub.substr(53, 11), nullptr, 16));
                bitset<40> bit_y(stoll(sub.substr(63, 10), nullptr, 16));
                bitset<44> bit_z(stoll(sub.substr(72, 11), nullptr, 16));

                int64_t ecef_X, ecef_Y, ecef_Z;
                string p_x, p_y, p_z;

                p_x = bit_x.to_string().substr(3, 38);
                p_y = bit_y.to_string().substr(1,38);
                p_z = bit_z.to_string().substr(3, 38);

                if(p_x.substr(0, 1)=="1"){
                    int64_t data_filed = int64_t(stoll(p_x.substr(1, 37), nullptr, 2));
                    ecef_X = ((data_filed^exclusive_or) + 1) * -1;
                }else{
                    ecef_X = int64_t (stoll(p_x, nullptr, 2));
                }
                if(p_y.substr(0, 1)=="1"){
                    int64_t data_filed = int64_t(stoll(p_y.substr(1, 37), nullptr, 2));
                    ecef_Y = ((data_filed^exclusive_or) + 1) * -1;
                }else{
                    ecef_Y = int64_t (stoll(p_y, nullptr, 2));
                }
                if (p_z.substr(0, 1)=="1"){
                    int64_t data_filed = int64_t (stoll(p_z.substr(1, 37), nullptr, 2));
                    ecef_Z = ((data_filed^exclusive_or) + 1) * -1;
                }else{
                    ecef_Z = int64_t (stoll(p_z, nullptr, 2));
                }
                // 缩放因子
                double ecef_x, ecef_y, ecef_z;
                ecef_x=double(ecef_X)/10000;
                ecef_y=double(ecef_Y)/10000;
                ecef_z=double(ecef_Z)/10000;

                bitset<36> bit_vx(stoll(sub.substr(82, 9), nullptr, 16));
                bitset<36> bit_vy(stoll(sub.substr(90, 9), nullptr, 16));
                bitset<36> bit_vz(stoll(sub.substr(98, 9), nullptr, 16));

                int32_t ecef_vX, ecef_vY, ecef_vZ;
                string v_x, v_y, v_z;

                v_x = bit_vx.to_string().substr(1, 32);
                v_y = bit_vy.to_string().substr(1, 32);
                v_z = bit_vz.to_string().substr(1, 32);
                ecef_vX = int32_t (stoll(v_x, nullptr, 2));
                ecef_vY = int32_t (stoll(v_y, nullptr, 2));
                ecef_vZ = int32_t (stoll(v_z, nullptr, 2));
                double ecef_vx, ecef_vy, ecef_vz;
                ecef_vx = double(ecef_vX)/1000000;
                ecef_vy = double(ecef_vY)/1000000;
                ecef_vz = double(ecef_vZ)/1000000;

                if(outputfile){
                    outputfile<<fixed<<setprecision(10)<<epoch<<","<<ecef_x<<","<<ecef_y<<","<<ecef_z<<","<<ecef_vx<<","<<ecef_vy<<","<<
                    ecef_vz<<endl;
                }

            }
        }catch( out_of_range & e  ){
            cout<<e.what()<<endl;
        }catch( invalid_argument & e2){
            cout<<e2.what()<<endl;
        }


        start_pos = end_pos;
        // 循环跳出
        if (start_pos == std::string::npos) {
            break;
        }
        end_pos = arr.find(split, start_pos + 4);
        delta = end_pos - start_pos;
        sub = arr.substr(start_pos, delta);

    }
    outputfile.close();
}