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

#include "Function1D.h"
#include "MatlabReader.h"
#include "Matrix.h"
#include "reflectionReconstruction/dataFilter.h"
#include "transmissionReconstruction/dataFilter/dataFilter.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 DataFilter_Test : public ::testing::Test {
protected:
    static void SetUpDataFilterTester() {

    }

    static void TearDownTestCase() {
    }

    void SetUp() {
    }

    void TearDown() {
    }
};

TEST_F(DataFilter_Test, filterTransmissionSensitivityMap) {
    double *dataA = new double[4]{1, 1, 1, 1};
    auto slBlock  = Aurora::Matrix::fromRawData(dataA, 4, 1);
    double *dataB = new double[4]{1, 1, 1, 1};
    auto snBlock  = Aurora::Matrix::fromRawData(dataB, 4, 1);
    double *data3 = new double[4]{1, 1, 1, 2};
    auto rlBlock   = Aurora::Matrix::fromRawData(data3, 4, 1);
    double *data4 = new double[4]{1, 2, 3, 1};
    auto rnBlock    = Aurora::Matrix::fromRawData(data4, 4, 1);
    std::vector<Aurora::Matrix> a66;
    double *data6 = new double[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_DOUBLE_EQ(1.0,result.getData()[i]);
    }
    
}

TEST_F(DataFilter_Test, filterTransmissionAngle) {
    double *dataA = new double[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));
    double *dataB = new double[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));
    double angleLowerLimit = 10;
    double angleUpperLimit = 180;

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

TEST_F(DataFilter_Test, checkTofDetections) {
    double *dataA = new double[3]{-3.0e-07, -2.6e-06, -2.6e-06};
    auto tofValues = Aurora::Matrix::fromRawData(dataA, 3, 1);
    double *dataB = new double[3]{0.238, 0.249, 0.249};
    auto dists = Aurora::Matrix::fromRawData(dataB, 3, 1);
    double *data3 = new double[3]{1476.35, 1476.28, 1476.28};
    auto sosRef = Aurora::Matrix::fromRawData(data3, 3, 1);
    double minSpeedOfSound = 1400;
    double maxSpeedOfSound = 1650;

    auto result = Recon::checkTofDetections(tofValues, dists, sosRef, minSpeedOfSound,maxSpeedOfSound);
    EXPECT_EQ(3,result.getDataSize());
    EXPECT_DOUBLE_AE(1479.1025,result.getData()[0]);
    EXPECT_DOUBLE_AE(1499.3931,result.getData()[1]);
    EXPECT_DOUBLE_AE(1499.3931,result.getData()[2]);
}

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_DOUBLE_AE(snrBlock.getData()[i],result.getData()[i]);
    }
}

TEST_F(DataFilter_Test, findDefectTransmissionData) {
    MatlabReader m("/home/krad/TestData/finddefect.mat");
    double *dataA = new double[3]{1, std::numeric_limits<double>::infinity(), -std::numeric_limits<double>::infinity()};
    auto SNRList = m.read("SNRList");
    auto SNRList2 = Aurora::Matrix::fromRawData(dataA, 3,1,1);
    auto result = m.read("valid");

    auto valid = Recon::findDefectTransmissionData(SNRList2,0.99);
    EXPECT_DOUBLE_AE(valid[0],1);
    EXPECT_DOUBLE_AE(valid[1],0);
    valid = Recon::findDefectTransmissionData(SNRList,0.99);
    for (size_t i = 0; i < valid.getDataSize(); i++)
    {
        EXPECT_DOUBLE_AE(valid[i],0);
    }
}

TEST_F(DataFilter_Test, filterReflectionData) {
    auto receiverPositionBlock  = Aurora::transpose(Aurora::Matrix::fromRawData(new double[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 double[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 double[9]{0.99,0.99,0.99,0.10,0.10,0.10,0,0,0}, 3, 3));
    int constrictReflectionAngles = 1;
    double lowerAngle = 45;
    double upperAngle = 360;
    auto result = Recon::filterReflectionData(receiverPositionBlock, senderPositionBlock, senderNormalBlock, constrictReflectionAngles, lowerAngle, upperAngle);
    EXPECT_EQ(3, result.getDataSize());
    EXPECT_DOUBLE_AE(result[0],1);
    EXPECT_DOUBLE_AE(result[1],1);
    EXPECT_DOUBLE_AE(result[2],1);

}