#include "data.h"

using namespace cv;

namespace uwb_slam{
    Data::Data():data_(5,0),predata_(5,0){
        odom_sub_ = nh_.subscribe("odom_imu_ekf",10,&Data::odomCB,this);
        fileout.open(filename);
        fileout1.open(filename1);
        fileout2.open(filename2);

        if(fileout.is_open()||fileout1.is_open()||fileout2.is_open()){
            std::cout<<"open file"<<std::endl;
        }else{
            std::cerr << "Unable to open file " << filename << std::endl;
        }    
    }

    void Data::Run(){
        
        while(!runL_->stop){
            if(imuDataRxTime!=odom_.header.stamp){
            if(uwb_->d[0]>2000||uwb_->d[1]>2000||uwb_->d[2]>2000||uwb_->d[0]<0){
                continue;
            }else{
            	if(x!=odom_.pose.pose.position.x*100||y!=odom_.pose.pose.position.y*100){
                    x = odom_.pose.pose.position.x*100;
                    y = odom_.pose.pose.position.y*100;
                    if(x==0&&y==0) continue;
                    data_[0] = x;
                    data_[1] = y;
                    data_[2] = uwb_->d[0];
                    data_[3] = uwb_->d[1];
                    data_[4] = uwb_->d[2];
                    std::cout<<"data:"<<uwb_->d[0]<<","<<uwb_->d[1]<<","<<uwb_->d[2]<<std::endl;
                    fileout1<<data_[0]<<","<<data_[1]<<","<<data_[2]<<","<<data_[3]<<","<<data_[4]<<"\n";
                    filter.LPF(data_,60);
                    if(predata_!=data_){
                        fileout2<<data_[0]<<","<<data_[1]<<","<<data_[2]<<","<<data_[3]<<","<<data_[4]<<"\n";
                        data_all.push_back(data_);  
                        predata_==data_;
                    }
                    imuDataRxTime=odom_.header.stamp;
                }
            }
            
                
            }
        }
        fileout1.flush();
        fileout1.close();
        fileout2.flush();
        fileout2.close();
        calculate(data_all);
    }

    void Data::calculate(std::vector<vector<float>> &data){
        int N = data.size();

        Mat S(N,3,CV_32F);          //每个时刻的位置坐标 N*3(xi,yi,zi)
        for(int i=0;i<N;i++){
            S.at<float>(i,0) = data[i][0];
            S.at<float>(i,1) = data[i][1];
            S.at<float>(i,2) = H_0;
        }

        Mat dist(tag_num,N,CV_32F); //每个标签的距离 tag_num*N
        for(int i=0;i<N;i++){
            dist.at<float>(0,i) = data[i][2];
            dist.at<float>(1,i) = data[i][3];
            dist.at<float>(2,i) = data[i][4];
        }

        Mat A(N,4,CV_32F);
        for(int i=0;i<N;i++){
            A.at<float>(i,0) = -2*S.at<float>(i,0);
            A.at<float>(i,1) = -2*S.at<float>(i,1);
            A.at<float>(i,2) = -2*S.at<float>(i,2);
            A.at<float>(i,3) = 1.0;
        }

        Mat k(N,1,CV_32F);
        for(int i=0;i<N;i++){
            k.at<float>(i,0) = pow(S.at<float>(i,0),2)+pow(S.at<float>(i,1),2)+pow(S.at<float>(i,2),2);
        }

        // Mat loc(tag_num,2,CV_32F);
        Mat ATA = A.t()*A;
        Mat ATA_inv = ATA.inv(DECOMP_SVD);

        for(int i=0;i<tag_num;i++){
            Mat y_m = Mat::zeros(N,1,CV_32F);
            for(int j=0;j<N;j++){
                y_m.at<float>(j,0) = pow(dist.at<float>(i,j),2)-k.at<float>(j,0);
            }
            Mat pos = ATA_inv*A.t()*y_m;
            // loc.at<float>(i,0) = pos.at<float>(0,0);
            // loc.at<float>(i,1) = pos.at<float>(1,0);
            // loc.at<float>(i,2) = H_tag;
            std::cout<<pos.at<float>(0,0)<<","<<pos.at<float>(1,0)<<","<<H_tag<<std::endl;
            fileout<<pos.at<float>(0,0)<<","<<pos.at<float>(1,0)<<","<<H_tag<<std::endl;
        }
        fileout.close();
    }

    void Data::odomCB(const nav_msgs::Odometry& odom){
        odom_ = odom;
    }
}
