#include <gtest/gtest.h>

#include "Matrix.h"
#include "common/ceMatchedFilterHandling.h"
#include "common/common.h"
#include "common/convertfp16tofloat.h"
#include "common/getGeometryInfo.h"
#include "common/daraBlockCreation/getAscanBlock.h"

#include "Parser.h"
#include "MatlabReader.h"

inline double fourDecimalRound(double src){
    return round(src*10000.0)/10000.0;
}

#define EXPECT_DOUBLE_AE(valueA,valueB)\
    EXPECT_DOUBLE_EQ(fourDecimalRound(valueA),fourDecimalRound(valueB))

class Common_Test : public ::testing::Test {
protected:
    static void SetUpCommonTester() {

    }

    static void TearDownTestCase() {
    }

    void SetUp() {
    }

    void TearDown() {
    }
};

TEST_F(Common_Test, adaptFrequency) {
    // MatlabReader m("/home/krad/TestData/adaptFrequency.mat");
    MatlabReader m("/home/sun/Aurora_back/build/a.mat");

    auto mCe = m.read("input");
    mCe = Recon::adaptFrequency(mCe,40000000.0,10000000.0);
    EXPECT_EQ(4096,mCe.getDataSize());
    auto output = m.read("output");
    for (size_t i = 0; i < 4096; i++)
    {
        EXPECT_DOUBLE_AE(mCe.getData()[i],output.getData()[i])<<", index:"<<i;
    }
    
}

TEST_F(Common_Test, convertfp16tofloat) {
    MatlabReader m("/home/krad/TestData/convertReal.mat");

    size_t count = 0;
    auto input = m.readint16("input",count);
    auto ma = Aurora::Matrix::copyFromRawData((double*)input.get(),count/4);
    auto resultM = Recon::convertfp16tofloat(ma);
    auto result = resultM.getData();
    auto output = m.read("output");
    for (size_t i = 0; i<count; i++) {
        EXPECT_EQ(result[i], output.getData()[i])<<"index:"<<i<<",input:"<< ((short*)ma.getData())[i]<<",input2:"<<input.get()[i];
    }

}

TEST_F(Common_Test, convertToLinearIndices) {
    //2D
    {
        double *dataA = new double[6]{1, 9, 4, 0, .01, 1.9};
        auto A = Aurora::Matrix::fromRawData(dataA, 3, 2);
        double *dataB = new double[2]{3, 2};
        auto B = Aurora::Matrix::fromRawData(dataB, 2, 1);
        auto output =Recon::convertToLinearIndices(B,A);
        EXPECT_DOUBLE_AE(-2,output.getData()[0]);
        EXPECT_DOUBLE_AE(6.03,output.getData()[1]);
        EXPECT_DOUBLE_AE(6.7,output.getData()[2]);
    }
    //3D
    {
        double *dataA = new double[12]{1, 9, 4, 0, .01, 1.9, 1, 9, 4, 0, .01, 1.9};
        auto A = Aurora::Matrix::fromRawData(dataA, 2, 3, 2);
        double *dataB = new double[3]{3, 2, 2};
        auto B = Aurora::Matrix::fromRawData(dataB, 3, 1);
        auto output =Recon::convertToLinearIndices(B,A);
        EXPECT_DOUBLE_AE(4.06,output.getData()[0]);
        EXPECT_DOUBLE_AE(11.4,output.getData()[1]);
    }


}

TEST_F(Common_Test, getGeometryInfo) {
     MatlabReader m("/home/sun/testData/getGeometryInfo.mat");
     auto correctReceiverNormals = m.read4d("receiverNormals");
     auto correctReceiverPositions = m.read4d("receiverPositions");
     auto correctSenderNormals = m.read4d("senderNormals");
     auto correctSenderPositions = m.read4d("senderPositions");
     auto motoPos = m.read("motorPosAvailable");
     auto transformationMatrices = m.read("transformationMatrices");
     auto rlList = m.read("rlList");
     auto rnList = m.read("rnList");
     auto slList = m.read("slList");
     auto snList = m.read("snList");
     auto geom = Recon::getGeometryInfo(motoPos,transformationMatrices,rlList,rnList,slList,snList);
     auto receiverNormals = geom.receiverNormals;
     auto receiverPositions = geom.receiverPositions;
     auto senderNormals = geom.senderNormals;
     auto senderPositions = geom.senderPositions;
     EXPECT_DOUBLE_EQ(correctReceiverNormals.size(), receiverNormals.size());
     for(int i=0;i<correctReceiverNormals.size();++i)
     {
        EXPECT_DOUBLE_EQ(correctReceiverNormals[i].getDimSize(0), receiverNormals[i].getDimSize(0));
        EXPECT_DOUBLE_EQ(correctReceiverNormals[i].getDimSize(1), receiverNormals[i].getDimSize(1));
        EXPECT_DOUBLE_EQ(correctReceiverNormals[i].getDimSize(2), receiverNormals[i].getDimSize(2));
        for(int j=0;j<correctReceiverNormals[i].getDataSize();++j)
        {
            EXPECT_DOUBLE_AE(correctReceiverNormals[i][j], receiverNormals[i][j]);
        }
     }

     EXPECT_DOUBLE_EQ(correctReceiverPositions.size(), receiverPositions.size());
     for(int i=0;i<correctReceiverPositions.size();++i)
     {
        EXPECT_DOUBLE_EQ(correctReceiverPositions[i].getDimSize(0), receiverPositions[i].getDimSize(0));
        EXPECT_DOUBLE_EQ(correctReceiverPositions[i].getDimSize(1), receiverPositions[i].getDimSize(1));
        EXPECT_DOUBLE_EQ(correctReceiverPositions[i].getDimSize(2), receiverPositions[i].getDimSize(2));
        for(int j=0;j<correctReceiverPositions[i].getDataSize();++j)
        {
            EXPECT_DOUBLE_AE(correctReceiverPositions[i][j], receiverPositions[i][j]);
        }
     }

     EXPECT_DOUBLE_EQ(correctSenderNormals.size(), senderNormals.size());
     for(int i=0;i<correctSenderNormals.size();++i)
     {
        EXPECT_DOUBLE_EQ(correctSenderNormals[i].getDimSize(0), senderNormals[i].getDimSize(0));
        EXPECT_DOUBLE_EQ(correctSenderNormals[i].getDimSize(1), senderNormals[i].getDimSize(1));
        EXPECT_DOUBLE_EQ(correctSenderNormals[i].getDimSize(2), senderNormals[i].getDimSize(2));
        for(int j=0;j<correctSenderNormals[i].getDataSize();++j)
        {
            EXPECT_DOUBLE_AE(correctSenderNormals[i][j], senderNormals[i][j]);
        }
     }          

     EXPECT_DOUBLE_EQ(correctSenderPositions.size(), senderPositions.size());
     for(int i=0;i<correctSenderPositions.size();++i)
     {
        EXPECT_DOUBLE_EQ(correctSenderPositions[i].getDimSize(0), senderPositions[i].getDimSize(0));
        EXPECT_DOUBLE_EQ(correctSenderPositions[i].getDimSize(1), senderPositions[i].getDimSize(1));
        EXPECT_DOUBLE_EQ(correctSenderPositions[i].getDimSize(2), senderPositions[i].getDimSize(2));
        for(int j=0;j<correctSenderPositions[i].getDataSize();++j)
        {
            EXPECT_DOUBLE_AE(correctSenderPositions[i][j], senderPositions[i][j]);
        }
     }  

    Aurora::Matrix sensChar = m.read("sensChar");
    EXPECT_DOUBLE_EQ(geom.sensChar.getDataSize(), sensChar.getDataSize());
    for(int i=0;i<geom.sensChar.getDataSize();++i)
    {
        EXPECT_DOUBLE_AE(geom.sensChar.getData()[i], sensChar.getData()[i]);
    }

    Aurora::Matrix headTable = m.read("headTable");
    EXPECT_DOUBLE_EQ(geom.headTable.getDataSize(), headTable.getDataSize());
    for(int i=0;i<geom.headTable.getDataSize();++i)
    {
        EXPECT_DOUBLE_AE(geom.headTable.getData()[i], headTable.getData()[i]);
    }    

    Aurora::Matrix minEmitter = m.read("minEmitter");
    EXPECT_DOUBLE_EQ(geom.minEmitter.getDataSize(), minEmitter.getDataSize());
    for(int i=0;i<geom.minEmitter.getDataSize();++i)
    {
        EXPECT_DOUBLE_AE(geom.minEmitter.getData()[i], minEmitter.getData()[i]);
    }  

    Aurora::Matrix minReceiver = m.read("minReceiver");
    EXPECT_DOUBLE_EQ(geom.minReceiver.getDataSize(), minReceiver.getDataSize());
    for(int i=0;i<geom.minReceiver.getDataSize();++i)
    {
        EXPECT_DOUBLE_AE(geom.minReceiver.getData()[i], minReceiver.getData()[i]);
    } 

    Aurora::Matrix minSize = m.read("minSize");
    EXPECT_DOUBLE_EQ(geom.minSize.getDataSize(), minSize.getDataSize());
    for(int i=0;i<geom.minSize.getDataSize();++i)
    {
        EXPECT_DOUBLE_AE(geom.minSize.getData()[i], minSize.getData()[i]);
    }   

     Aurora::Matrix maxEmitter = m.read("maxEmitter");
    EXPECT_DOUBLE_EQ(geom.maxEmitter.getDataSize(), maxEmitter.getDataSize());
    for(int i=0;i<geom.maxEmitter.getDataSize();++i)
    {
        EXPECT_DOUBLE_AE(geom.maxEmitter.getData()[i], maxEmitter.getData()[i]);
    }  

    Aurora::Matrix maxReceiver = m.read("maxReceiver");
    EXPECT_DOUBLE_EQ(geom.maxReceiver.getDataSize(), maxReceiver.getDataSize());
    for(int i=0;i<geom.maxReceiver.getDataSize();++i)
    {
        EXPECT_DOUBLE_AE(geom.maxReceiver.getData()[i], maxReceiver.getData()[i]);
    } 

    Aurora::Matrix maxSize = m.read("maxSize");
    EXPECT_DOUBLE_EQ(geom.maxSize.getDataSize(), maxSize.getDataSize());
    for(int i=0;i<geom.maxSize.getDataSize();++i)
    {
        EXPECT_DOUBLE_AE(geom.maxSize.getData()[i], maxSize.getData()[i]);
    }

    Aurora::Matrix maxSL = m.read("maxSL");
    EXPECT_DOUBLE_EQ(geom.maxSL, maxSL[0]);         

    Aurora::Matrix maxSN = m.read("maxSN");
    EXPECT_DOUBLE_EQ(geom.maxSN, maxSN[0]);   

    Aurora::Matrix maxRL = m.read("maxRL");
    EXPECT_DOUBLE_EQ(geom.maxRL, maxRL[0]);    

    Aurora::Matrix maxRN = m.read("maxRN");
    EXPECT_DOUBLE_EQ(geom.maxRN, maxRN[0]);
}

TEST_F(Common_Test, getAscanBlock) {
    MatlabReader m("/home/sun/testData/getAscanBlock.mat");
    std::string dataPath = "/home/AScans_Data/volunteer_20230328/20230328T123237/";
    Parser parser(dataPath);
    auto ascanBlock = m.read("AscanBlock");
    auto result = Recon::getAscanBlock(&parser, Aurora::Matrix::fromRawData(new double[1]{1}, 1), Aurora::Matrix::fromRawData(new double[1]{1}, 1),
                                        Aurora::Matrix::fromRawData(new double[18]{1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16,17,18}, 18),
                                        Aurora::Matrix::fromRawData(new double[1]{1}, 1),
                                        Aurora::Matrix::fromRawData(new double[18]{1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16,17,18}, 18));
    for(int i=0; i<ascanBlock.getDataSize(); ++i)
    {
        EXPECT_DOUBLE_AE(ascanBlock[i], result.ascanBlock[i]);
    }
    EXPECT_DOUBLE_EQ(ascanBlock.getDimSize(0), result.ascanBlock.getDimSize(0));
    EXPECT_DOUBLE_EQ(ascanBlock.getDimSize(1), result.ascanBlock.getDimSize(1));

    auto gainBlock = m.read("gainBlock");
    for(int i=0; i<gainBlock.getDataSize(); ++i)
    {
        EXPECT_DOUBLE_AE(gainBlock[i], result.gainBlock[i]);
    }
    EXPECT_DOUBLE_EQ(gainBlock.getDimSize(0), result.gainBlock.getDimSize(0));
    EXPECT_DOUBLE_EQ(gainBlock.getDimSize(1), result.gainBlock.getDimSize(1));

    auto mpBlock = m.read("mpBlock");
    for(int i=0; i<mpBlock.getDataSize(); ++i)
    {
        EXPECT_DOUBLE_AE(mpBlock[i], result.mpBlock[i]);
    }
    EXPECT_DOUBLE_EQ(mpBlock.getDimSize(0), result.mpBlock.getDimSize(0));
    EXPECT_DOUBLE_EQ(mpBlock.getDimSize(1), result.mpBlock.getDimSize(1));

    auto rlBlock = m.read("rlBlock");
    for(int i=0; i<rlBlock.getDataSize(); ++i)
    {
        EXPECT_DOUBLE_AE(rlBlock[i], result.rlBlock[i]);
    }
    EXPECT_DOUBLE_EQ(rlBlock.getDimSize(0), result.rlBlock.getDimSize(0));
    EXPECT_DOUBLE_EQ(rlBlock.getDimSize(1), result.rlBlock.getDimSize(1));

    auto rnBlock = m.read("rnBlock");
    for(int i=0; i<rnBlock.getDataSize(); ++i)
    {
        EXPECT_DOUBLE_AE(rnBlock[i], result.rnBlock[i]);
    }
    EXPECT_DOUBLE_EQ(rnBlock.getDimSize(0), result.rnBlock.getDimSize(0));
    EXPECT_DOUBLE_EQ(rnBlock.getDimSize(1), result.rnBlock.getDimSize(1));

    auto slBlock = m.read("slBlock");
    for(int i=0; i<slBlock.getDataSize(); ++i)
    {
        EXPECT_DOUBLE_AE(slBlock[i], result.slBlock[i]);
    }
    EXPECT_DOUBLE_EQ(slBlock.getDimSize(0), result.slBlock.getDimSize(0));
    EXPECT_DOUBLE_EQ(slBlock.getDimSize(1), result.slBlock.getDimSize(1));

    auto snBlock = m.read("snBlock");
    for(int i=0; i<snBlock.getDataSize(); ++i)
    {
        EXPECT_DOUBLE_AE(snBlock[i], result.snBlock[i]);
    }
    EXPECT_DOUBLE_EQ(snBlock.getDimSize(0), result.snBlock.getDimSize(0));
    EXPECT_DOUBLE_EQ(snBlock.getDimSize(1), result.snBlock.getDimSize(1));
}