#include <gtest/gtest.h>
#include <limits>

#include "Function1D.h"
#include "MatlabReader.h"
#include "Matrix.h"
#include "config/config.h"
#include "reflectionReconstruction/dataFilter.h"
#include "transmissionReconstruction/dataFilter/dataFilter.h"
#include "src/transmissionReconstruction/dataPreperation.h"
#include "src/common/getMeasurementMetaData.h"



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

#define EXPECT_FLOAT_AE(valueA,valueB)\
    EXPECT_FLOAT_EQ(fourDecimalRound(valueA),fourDecimalRound(valueB))

class DataFilter_Test : public ::testing::Test {
protected:
    static void SetUpDataFilterTester() {

    }

    static void TearDownTestCase() {
    }

    void SetUp() {
    }

    void TearDown() {
    }
};

TEST_F(DataFilter_Test, filterTransmissionSensitivityMap) {
    float *dataA = new float[4]{1, 1, 1, 1};
    auto slBlock  = Aurora::Matrix::fromRawData(dataA, 4, 1);
    float *dataB = new float[4]{1, 1, 1, 1};
    auto snBlock  = Aurora::Matrix::fromRawData(dataB, 4, 1);
    float *data3 = new float[4]{1, 1, 1, 2};
    auto rlBlock   = Aurora::Matrix::fromRawData(data3, 4, 1);
    float *data4 = new float[4]{1, 2, 3, 1};
    auto rnBlock    = Aurora::Matrix::fromRawData(data4, 4, 1);
    std::vector<Aurora::Matrix> a66;
    float *data6 = new float[6]{3, 2, 1, 9, 8, 6};
    auto sensData0 = Aurora::Matrix::fromRawData(data6, 3, 2, 1);
    a66.push_back(sensData0);
    auto result = Recon::filterTransmissionSensitivityMap(0.3, slBlock, snBlock, rlBlock, rnBlock, a66);
    EXPECT_EQ(4,result.getDataSize());
    for (size_t i = 0; i < 4; i++)
    {
        EXPECT_FLOAT_EQ(1.0,result.getData()[i]);
    }
    
}

TEST_F(DataFilter_Test, filterTransmissionAngle) {
    float *dataA = new float[12]{0.99,0.99,0.99,0.99,0.10,0.10,0.10,0.10,0,0,0,0};
    auto senderNormalBlock   = Aurora::transpose(Aurora::Matrix::fromRawData(dataA, 4, 3));
    float *dataB = new float[12]{0.99,0.99,0.99,0.98,0.10,0.10,0.10,-0.15,0,0,0,0};
    auto receiverNormalBlock   = Aurora::transpose(Aurora::Matrix::fromRawData(dataB, 4, 3));
    float angleLowerLimit = 10;
    float angleUpperLimit = 180;

    auto result = Recon::filterTransmissionAngle(angleLowerLimit, angleUpperLimit, senderNormalBlock, receiverNormalBlock);
    EXPECT_EQ(4,result.getDataSize());
    EXPECT_FLOAT_EQ(0.0,result.getData()[0]);
    EXPECT_FLOAT_EQ(0.0,result.getData()[1]);
    EXPECT_FLOAT_EQ(0.0,result.getData()[2]);
    EXPECT_FLOAT_EQ(1.0,result.getData()[3]);
}


// TEST_F(DataFilter_Test, checkTofDetections) {
//     MatlabReader m("/home/sun/testData/checkTofDetections.mat");
//     auto receiverList = m.read("receiverList");
//     auto senderList = m.read("senderList");
//     auto tofDataTotal = m.read("tofDataTotal");
//     auto waterTempList = m.read("waterTempList");
//     auto tofValues = m.read("tofValues");
//     auto valid = m.read("valid");
//     Aurora::Matrix dists = Recon::distanceBetweenTwoPoints(senderList, receiverList);
//     Aurora::Matrix sosRef = Recon::temperatureToSoundSpeed(waterTempList, "marczak");
//     auto result = Recon::checkTofDetections(tofDataTotal, dists, sosRef, Recon::transParams::minSpeedOfSound,Recon::transParams::maxSpeedOfSound);

//     for (size_t i = 0; i < result.valid.getDataSize(); i++)
//     {
//         EXPECT_FLOAT_AE(valid.getData()[i],result.valid.getData()[i]) << " :"<<i;
//     }

//     for (size_t i = 0; i < result.tofValues.getDataSize(); i++)
//     {
//         EXPECT_FLOAT_AE(tofValues.getData()[i],result.tofValues.getData()[i])<< " :"<<i;
//     }
// }

TEST_F(DataFilter_Test, calculateSnr) {
    MatlabReader m("/home/krad/TestData/snr.mat");

    auto snrBlock = m.read("snrBlock");
    auto dataBlock = m.read("dataBlock");
    auto result = Recon::calculateSnr(dataBlock,1.978);

    for (size_t i = 0; i < snrBlock.getDataSize(); i++)
    {
        EXPECT_FLOAT_AE(snrBlock.getData()[i],result.getData()[i]);
    }
}

TEST_F(DataFilter_Test, findDefectTransmissionData) {
    MatlabReader m("/home/sun/testData/findDefectTransmissionData.mat");
    float *dataA = new float[3]{1, std::numeric_limits<float>::infinity(), -std::numeric_limits<float>::infinity()};
    auto SNRList = m.read("vsnrlist");
    auto SNRList2 = m.read("snrdifference");
    auto validOut = m.read("validOut");

    auto valid1 = Recon::findDefectTransmissionData(SNRList,Recon::transParams::snrThreshold);
    auto valid2 = Recon::findDefectTransmissionData(SNRList2,Recon::transParams::snrThreshold);
    auto valid = valid1*valid2;
    for (size_t i = 0; i < valid.getDataSize(); i++)
    {
        EXPECT_FLOAT_AE(validOut[i],valid[i]);
    }
}

TEST_F(DataFilter_Test, filterReflectionData) {
    auto receiverPositionBlock  = Aurora::transpose(Aurora::Matrix::fromRawData(new float[9]{-0.12,-0.12,-0.12,-0.01,-0.01,-0.01,0.03,0.03,0.04}, 3, 3));
    auto senderPositionBlock = Aurora::transpose(Aurora::Matrix::fromRawData(new float[9]{-0.12,-0.12,-0.12,-0.01,-0.01,-0.01,0.03,0.03,0.03}, 3, 3));
    auto senderNormalBlock  = Aurora::transpose(Aurora::Matrix::fromRawData(new float[9]{0.99,0.99,0.99,0.10,0.10,0.10,0,0,0}, 3, 3));
    int constrictReflectionAngles = 1;
    auto result = Recon::filterReflectionData(receiverPositionBlock, senderPositionBlock, senderNormalBlock, constrictReflectionAngles);
    EXPECT_EQ(3, result.getDataSize());
    EXPECT_FLOAT_AE(result[0],1);
    EXPECT_FLOAT_AE(result[1],1);
    EXPECT_FLOAT_AE(result[2],0);

}