#include <gtest/gtest.h>

#include "Matrix.h"
#include "common/getGeometryInfo.h"
#include "transmissionReconstruction/dataFilter/sensitivityCalculations.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))

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

class Sensitivity_Test : public ::testing::Test {
protected:
    static void SetUpSensitivityTester() {

    }

    static void TearDownTestCase() {
    }

    void SetUp() {
    }

    void TearDown() {
    }
};

#include "Function3D.h"

TEST_F(Sensitivity_Test, getSensitivity) {
    MatlabReader m("/home/krad/TestData/sensitivity.mat");

    auto sensmap = m.read("sensmap");
    auto senderNormal = m.read("senderNormal");
    auto dirToReceiver = m.read("dirVector");
    {    auto xyn = Aurora::zeros(1, 3);
    xyn.getData()[2] = 1;
    xyn = Aurora::repmat(xyn,2304, 1);
    }

    auto output = Recon::getSensitivity(sensmap, senderNormal, dirToReceiver);

    auto sens = m.read("sens");
    EXPECT_EQ(output.getDataSize(), sens.getDataSize());
    for (size_t i = 0; i < output.getDataSize(); i++)
    {
        EXPECT_DOUBLE_AE(output.getData()[i],sens.getData()[i])<<", index:"<<i;
    }
    
}

TEST_F(Sensitivity_Test, precalcSensitivityForTAS){
    MatlabReader m("/home/krad/TestData/sensTAS.mat");

    auto sensmap = m.read("sensChar");
    auto senderNormals = m.read("senderNormals");
    auto senderPositions = m.read("senderPositions");
    auto receiverPositions = m.read("receiverPositions");


    auto output = Recon::precalcSensitivityForTAS(sensmap, senderPositions,senderNormals, receiverPositions);

    auto sens = m.read("TASResult");
    EXPECT_EQ(output.getDataSize(), sens.getDataSize());
    for (size_t i = 0; i < output.getDataSize(); i++)
    {
        EXPECT_DOUBLE_AE(output.getData()[i],sens.getData()[i])<<", index:"<<i;
    }
}

TEST_F(Sensitivity_Test, combineSensitivity){
    MatlabReader m("/home/krad/TestData/combine.mat");

    auto senderTASRange = m.read("senderTASRange");
    auto senderElementRange = m.read("senderElementRange");
    auto receiverTASRange = m.read("receiverTASRange");
    auto receiverElementRange = m.read("receiverElementRange");
    auto senderSens = m.read("TASResult");
    auto receiverSens = m.read("TASResult2");


    auto output = Recon::combineSensitivity(senderTASRange, senderElementRange, receiverTASRange, receiverElementRange, senderSens, receiverSens);

    auto sens = m.read4d("combineResult");
    EXPECT_EQ(output.size(), sens.size());
    EXPECT_EQ(output[0].getDataSize(), sens[0].getDataSize());

    #pragma omp parallel for
    for (size_t i = 0; i < output.size(); i++)
    {
        for (size_t j = 0; j < output[i].getDataSize(); j++)
        {
            EXPECT_DOUBLE_AE(output[i].getData()[j],sens[i].getData()[j])<<"Matrix index:"<<i<<", value index:"<<j;
            
        }
    }
}

TEST_F(Sensitivity_Test,precalcSensitivity){
    MatlabReader m("/home/krad/TestData/precalcSensitivity.mat");

    auto senderTASRange = m.read("senderTASRange");
    auto senderElementRange = m.read("senderElementRange");
    auto receiverTASRange = m.read("receiverTASRange");
    auto receiverElementRange = m.read("receiverElementRange");
    MatlabReader m3("/home/krad/TestData/precalcSensitivity2.mat");
    Recon::GeometryInfo TASElements;
    TASElements.senderPositions = m3.read4d("senderPositions");
    TASElements.senderNormals = m3.read4d("senderNormals");
    TASElements.receiverPositions = m3.read4d("receiverPositions");
    TASElements.receiverNormals = m3.read4d("receiverNormals");
    TASElements.sensChar = m.read("sensChar");
    auto motorPos = Aurora::Matrix::fromRawData(new double[2]{1,2}, 2);
    auto output = precalcSensitivity(senderTASRange, senderElementRange, receiverTASRange, receiverElementRange, motorPos, TASElements);
    MatlabReader m2("/home/krad/TestData/precalcResult.mat");
    auto sens = m2.read4d("result");
    EXPECT_EQ(output.size(), sens.size());
    EXPECT_EQ(output[0].getDataSize(), sens[0].getDataSize());
    #pragma omp parallel for
    for (size_t i = 0; i < output.size(); i++)
    {
        for (size_t j = 0; j < output[i].getDataSize(); j++)
        {
            EXPECT_DOUBLE_AE(output[i].getData()[j],sens[i].getData()[j])<<"Matrix index:"<<i<<", value index:"<<j;
            
        }
    }
}