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

#include "Function1D.h"
#include "MatlabReader.h"
#include "Matrix.h"
#include "config/config.h"
#include "transmissionReconstruction/detection/detection.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))

#define ASSERT_DOUBLE_AE(valueA,valueB)\
    ASSERT_DOUBLE_EQ(fourDecimalRound(valueA),fourDecimalRound(valueB))

class Detection_Test : public ::testing::Test {
protected:
    static void SetUpDetectionTester() {

    }

    static void TearDownTestCase() {
    }

    void SetUp() {
    }

    void TearDown() {
    }
};

TEST_F(Detection_Test, detectTofAndAttMex) {

    MatlabReader m("/home/krad/TestData/getBlockOfTransmissionData.mat");

    auto AscanBlock = m.read("AscanBlock");
    auto AscanRefBlock = m.read("AscanRefBlock");
    auto distBlock = m.read("dists");
    auto distBlockRef = m.read("distRefBlock");
    auto sosWaterBlock = m.read("waterTempBlock");
    auto sosWaterRefBlock = m.read("waterTempRefBlock");
    double expectedSOSWater = 1.511948131508464e+03;

    auto result = Recon::detectTofAndAttMex(
        AscanBlock, AscanRefBlock, distBlock, distBlockRef, sosWaterBlock,
        sosWaterRefBlock, Recon::transParams::resampleFactor,
        Recon::transParams::nThreads, expectedSOSWater,
        Recon::transParams::useTimeWindowing,
        Recon::transParams::aScanReconstructionFrequency,
        Recon::transParams::offsetElectronic,Recon::transParams::detectionWindowATT,
        Recon::transParams::detectionWindowSOS,
        Recon::transParams::minSpeedOfSound,
        Recon::transParams::maxSpeedOfSound, Recon::transParams::gaussWindow);

    MatlabReader m2("/home/krad/TestData/sosResult.mat");
    auto sosvalue = m2.read("sosValue");
    ASSERT_EQ(sosvalue.getDataSize(), result.tof.getDataSize());
    ASSERT_EQ(sosvalue.getDataSize(), result.sosValue.getDataSize());
    ASSERT_EQ(sosvalue.getDataSize(), result.att.getDataSize());
    #pragma omp parallel for
    for (size_t i = 0; i < result.tof.getDataSize(); i++)
    {
        EXPECT_DOUBLE_AE(0,result.tof[i])<<",index:"<<i;
        EXPECT_DOUBLE_AE(sosvalue[i],result.sosValue[i])<<",index:"<<i;
        EXPECT_TRUE(std::isnan(result.att[i]))<<",index:"<<i;
    }

}

TEST_F(Detection_Test, detectTofAndAtt) {

    MatlabReader m("/home/krad/TestData/getBlockOfTransmissionData.mat");

    auto AscanBlock = m.read("AscanBlock");
    auto AscanRefBlock = m.read("AscanRefBlock");
    auto distBlock = m.read("dists");
    auto distBlockRef = m.read("distRefBlock");
    auto sosWaterBlock = m.read("waterTempBlock");
    auto sosWaterRefBlock = m.read("waterTempRefBlock");
    double expectedSOSWater = 1.511948131508464e+03;

    auto result = Recon::detectTofAndAtt(
        AscanBlock, AscanRefBlock, distBlock, distBlockRef, sosWaterBlock,
        sosWaterRefBlock, Recon::transParams::resampleFactor,
        Recon::transParams::nThreads, expectedSOSWater,
        Recon::transParams::useTimeWindowing,
        Recon::transParams::aScanReconstructionFrequency,
        Recon::transParams::offsetElectronic,Recon::transParams::detectionWindowATT,
        Recon::transParams::detectionWindowSOS,
        Recon::transParams::minSpeedOfSound,
        Recon::transParams::maxSpeedOfSound, Recon::transParams::gaussWindow);

    MatlabReader m2("/home/krad/TestData/sosResult.mat");
    auto sosvalue = m2.read("sosValue");
    ASSERT_EQ(sosvalue.getDataSize(), result.tof.getDataSize());
    ASSERT_EQ(sosvalue.getDataSize(), result.sosValue.getDataSize());
    ASSERT_EQ(sosvalue.getDataSize(), result.att.getDataSize());
    #pragma omp parallel for
    for (size_t i = 0; i < result.tof.getDataSize(); i++)
    {
        EXPECT_DOUBLE_AE(0,result.tof[i])<<",index:"<<i;
        EXPECT_DOUBLE_AE(sosvalue[i],result.sosValue[i])<<",index:"<<i;
        EXPECT_TRUE(std::isnan(result.att[i]))<<",index:"<<i;
    }

}

TEST_F(Detection_Test, detectAttVectorized) {

    MatlabReader m("/home/krad/TestData/getBlockOfTransmissionData.mat");

    auto AscanBlock = m.read("AscanBlock");
    auto AscanRefBlock = m.read("AscanRefBlock");
    auto distBlockRef = m.read("distRefBlock");
    auto sosWaterRefBlock = m.read("waterTempRefBlock");
    MatlabReader m2("/home/krad/TestData/tofResult.mat");
    auto tof = m2.read("tof");

    double expectedSOSWater = 1.511948131508464e+03;

    auto result = Recon::detectAttVectorized(
        AscanBlock, AscanRefBlock, distBlockRef,sosWaterRefBlock,
        tof,
        Recon::transParams::aScanReconstructionFrequency,
        Recon::transParams::offsetElectronic,
        Recon::transParams::detectionWindowSOS);
    for (size_t i = 0; i < result.getDataSize(); i++)
    {
        EXPECT_TRUE(std::isnan(result[i]))<<",index:"<<i;
    }
}

TEST_F(Detection_Test, calculateStarEndSearchPosition) {

    auto distBlock   = Aurora::Matrix::fromRawData(new double[3]{0.22, 0.21, 0.11}, 3, 1);
    auto sosOffsetBlock  = Aurora::Matrix::fromRawData(new double[3]{-0.8, 0, 0.9}, 3, 1);

    auto result = Recon::calculateStarEndSearchPosition(distBlock, 1400.0, 1650.0, 10000000, 9999, sosOffsetBlock,97.3,250);
    EXPECT_EQ(3,result.endSearch.getDataSize());
    EXPECT_EQ(3,result.startSearch.getDataSize());
    EXPECT_DOUBLE_AE(1429,result.startSearch[0]);
    EXPECT_DOUBLE_AE(1370,result.startSearch[1]);
    EXPECT_DOUBLE_AE(764,result.startSearch[2]);
    EXPECT_DOUBLE_AE(1918,result.endSearch[0]);
    EXPECT_DOUBLE_AE(1848,result.endSearch[1]);
    EXPECT_DOUBLE_AE(1134,result.endSearch[2]);
    
}

TEST_F(Detection_Test, calculateAttenuation) {

    MatlabReader m("/home/krad/TestData/calcAtt.mat");

    auto ascans = m.read("ascans");
    auto ascansRef = m.read("ascansRef");
    auto endPos = m.read("endPos");
    auto endPosRef = m.read("endPosRef");
    auto startPos = m.read("startPos");
    auto startPosRef = m.read("startPosRef");
    auto att = m.read("att");
    auto result = Recon::calculateAttenuation(ascans, startPos, endPos, ascansRef, startPosRef, endPosRef);
    for (size_t i = 0; i < att.getDataSize(); i++)
    {
        EXPECT_DOUBLE_AE(att[i],result[i]);
    }
}

TEST_F(Detection_Test, applyTimeWindowing) {

    MatlabReader m("/home/krad/TestData/timeWindow2.mat");

    auto AscanBlock = m.read("AscanBlock");
    auto distBlock = m.read("dists");
    auto sosBlock = m.read("waterTempBlock");
    auto AscanBlockProcessed = m.read("AscanBlock1");
    auto startSearch = m.read("startSearch");
    auto result = Recon::applyTimeWindowing(AscanBlock, 10000000, distBlock, sosBlock, 1.511948131508464e+03, 5.2, 1,  1450, 1550, false);
    #pragma omp parallel for
    for (size_t i = 0; i < AscanBlockProcessed.getDataSize(); i++)
    {
        EXPECT_DOUBLE_AE(AscanBlockProcessed[i],result.AscanBlockProcessed[i])<<",index:"<<i;
    }
 
}

TEST_F(Detection_Test, detectTofVectorized) {

    MatlabReader m("/home/krad/TestData/getBlockOfTransmissionData.mat");

    auto AscanBlock = m.read("AscanBlock");
    auto AscanRefBlock = m.read("AscanRefBlock");
    auto distBlock = m.read("dists");
    auto distBlockRef = m.read("distRefBlock");
    auto sosWaterBlock = m.read("waterTempBlock");
    auto sosWaterRefBlock = m.read("waterTempRefBlock");
    double expectedSOSWater = 1.511948131508464e+03;

    auto result = Recon::detectTofVectorized(
        AscanBlock, AscanRefBlock, distBlock, distBlockRef, sosWaterBlock,
        sosWaterRefBlock, expectedSOSWater,
        Recon::transParams::useTimeWindowing,
        Recon::transParams::aScanReconstructionFrequency,
        Recon::transParams::offsetElectronic,
        Recon::transParams::detectionWindowSOS,
        Recon::transParams::minSpeedOfSound,
        Recon::transParams::maxSpeedOfSound, Recon::transParams::gaussWindow);

    MatlabReader m2("/home/krad/TestData/tofResult.mat");

    auto tof = m2.read("tof");
    auto sosvalue = m2.read("sosValue");
    EXPECT_EQ(tof.getDataSize(), result.tof.getDataSize());
    #pragma omp parallel for
    for (size_t i = 0; i < tof.getDataSize(); i++)
    {
        EXPECT_DOUBLE_AE(tof[i],result.tof[i])<<",index:"<<i;
    }
}