#include <iostream>
#include <fstream>
#include <iomanip>
#include "LSQ_Iteration.h"
#include "KF_Prediction.h"
#include "Control_LQG.h"
#include "Input_X.h"
using namespace std;
const double v_light = 299792458;

int main()
{
    // LSQ_Iteration lsq;
    KF_Prediction KF_pre;
    // Control_LQG lqg;
    // lqg.get_PPS_offset();
    // double Time[23] = {1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16,17,18,19,20,21,22,23};
    // double offset[23] = {-280.399,
    //                     -281.772,
    //                     -282.951,
    //                     -284.189,
    //                     -285.515,
    //                     -286.801,
    //                     -287.999,
    //                     -289.313,
    //                     -290.630,
    //                     -292.199,
    //                     -293.725,
    //                     -295.074,
    //                     -296.338,
    //                     -297.610,
    //                     -298.867,
    //                     -300.036,
    //                     -301.263,
    //                     -302.705,
    //                     -304.422,
    //                     -306.182,
    //                     -308.053,
    //                     -309.771,
    //                     -311.058
    //                 };
    // double pre[19]={0.0};
    ifstream clockin_f("../PPSCtlIn1.dat");
    ofstream clockout_f("../PPSCtlOut2.dat");
    string str;
    if(!clockin_f){
        cout<<"open file fail!"<<endl;
        return 1;
    }
    while( getline(clockin_f,str))
    {
        double Time = atof(str.c_str());
        int X[4];
        double SM[3], MS[3];

        sscanf(str.c_str(), "%lf %d %d %d %d %lf %lf %lf %lf %lf %lf", &Time, X, X+1, X+2, X+3, SM, SM+1, SM+2, MS, MS+1, MS+2);
        // if(abs(Time-25717)<1e-6 || abs(Time-13857)<1e-6){
        //     cout<<Time;
        // }
        Input_X meas;
        meas.Clock_offset_Prange = (SM[0]-MS[0])/2/v_light*1e9;
        meas.fre_offset_Doppler = -(SM[2]);
        meas.Time = Time;
        KF_pre.InputClockOffset(meas);
        
        double pre_clock = KF_pre.PredictionClockOffset(Time+2);
        clockout_f<<std::fixed<<setprecision(4)<<Time+2<<"\t"<<pre_clock<<endl;
    }
    clockin_f.close();
    clockout_f.close();

    // for (size_t i = 0; i < 6; i++)
    // {
    //     KF_pre.InputClockOffset(Time[i], offset[i]);
    // }
    // for (size_t i = 10; i < 19; i++)
    // {
    //     pre[i] = KF_pre.PredictionClockOffset(Time[i]);
    // }
    // cout<<pre[3] <<pre[4] <<pre[5] <<endl;
    return 1;
}