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

#include "Function1D.h"
#include "MatlabReader.h"
#include "Matrix.h"
#include "config/config.h"
#include "common/getMeasurementMetaData.h"
#include "transmissionReconstruction/detection/detection.h"




inline float fourDecimalRound(float 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, detectTofAndAtt) {

//     MatlabReader m("/home/sun/testData/transmissionDetection.mat");

//     auto AscanBlock = m.read("AscanBlock");
//     auto AscanRefBlock = m.read("AscanRefBlock");
//     auto distBlock = m.read("dists");
//     auto distBlockRef = m.read("distRefBlock");
//     auto sosWaterBlock = Recon::temperatureToSoundSpeed(m.read("waterTempBlock"), "marczak");
//     auto sosWaterRefBlock = Recon::temperatureToSoundSpeed(m.read("waterTempRefBlock"), "marczak");
//     float expectedSOSWater = 1.512677498767504e+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::detectionWindowATT,Recon::transParams::offsetElectronic,
//         Recon::transParams::detectionWindowSOS,
//         Recon::transParams::minSpeedOfSound,
//         Recon::transParams::maxSpeedOfSound, Recon::transParams::gaussWindow);

//     MatlabReader m2("/home/krad/TestData/sosResult.mat");
//     auto sosvalue = m2.read("sosValue");
//     auto tof = m2.read("tofRel");
//     auto att = m2.read("att");
//     // auto result1 = Recon::detectTofAndAttMex(
//     //     AscanBlock, AscanRefBlock, distBlock, distBlockRef, sosWaterBlock,
//     //     sosWaterRefBlock, Recon::transParams::resampleFactor,
//     //     Recon::transParams::nThreads, expectedSOSWater,
//     //     Recon::transParams::useTimeWindowing,
//     //     Recon::transParams::aScanReconstructionFrequency,
//     //     Recon::transParams::detectionWindowATT,Recon::transParams::offsetElectronic,
//     //     Recon::transParams::detectionWindowSOS,
//     //     Recon::transParams::minSpeedOfSound,
//     //     Recon::transParams::maxSpeedOfSound, Recon::transParams::gaussWindow);

//     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(tof[i],result.tof[i])<<",index:"<<i;
//         EXPECT_DOUBLE_AE(sosvalue[i],result.sosValue[i])<<",index:"<<i;
//         // EXPECT_DOUBLE_AE(result1.att[i],result.att[i])<<",index:"<<i;
//         // EXPECT_TRUE((std::abs(att[i]-result.att[i])<0.001))<<",att["<<i<<"]"<<att[i]<<", res att["<<i<<"]"<<result.att[1];
//     }

// }

// 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");

//     float 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 float[3]{0.22, 0.21, 0.11}, 3, 1);
    auto sosOffsetBlock  = Aurora::Matrix::fromRawData(new float[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");
//     float 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;
//     }
// }