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

#include "Function1D.h"
#include "MatlabReader.h"
#include "MatlabWriter.h"
#include "Matrix.h"
#include "Sparse.h"
#include "config/config.h"
#include "transmissionReconstruction/reconstruction/buildMatrix/DGradient.h"
#include "transmissionReconstruction/reconstruction/buildMatrix/FMM.h"
#include "transmissionReconstruction/reconstruction/buildMatrix/buildMatrix.h"
#include "transmissionReconstruction/reconstruction/reconstruction.h"
#include "transmissionReconstruction/reconstruction/solvingEquationSystem/TVAL/TVAL.h"

#include "reflectionReconstruction/preprocessData/determineOptimalPulse.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 Reconstruction_Test : public ::testing::Test {
protected:
    static void SetUpReconstructionTester() {

    }

    static void TearDownTestCase() {
    }

    void SetUp() {
    }

    void TearDown() {
    }
};

TEST_F(Reconstruction_Test, determineOptimalPulse) {
    Recon::reflectParams::imageResolution = 8.6381e-04;
    Recon::reflectParams::optPulseFactor = 24;
    auto result = Recon::determineOptimalPulse(1.0000e-07,3000);
    EXPECT_EQ(3000, result.getDataSize());
    ASSERT_DOUBLE_AE(result[2], 0.0025);
    ASSERT_DOUBLE_AE(result[4], 0.0078);

    // for(size_t i=0; i<result.getDataSize(); ++i)
    // {
    //     ASSERT_DOUBLE_AE(f1[i], result.outSOS[i])<<"index:"<<i;
    // }
}

TEST_F(Reconstruction_Test, reconstructArt) {
    MatlabReader m("/home/sun/testData/reconstructArt.mat");
    auto data  = m.read("data");
    auto dataAtt  = m.read("dataAtt");
    auto dims = m.read("dims");
    auto receiverList  = m.read("receiverList");
    auto res = m.read("res");
    auto senderList  = m.read("senderList");
    auto SOS_IN_WATER  = m.read("SOS_IN_WATER");
    
    MatlabReader m2("/home/krad/TestData/gpuresult.mat");
    auto f1 = m2.read("out");
    auto result = Recon::reconstructArt(data, dataAtt, dims, senderList, receiverList, res, SOS_IN_WATER[0]);
    for(size_t i=0; i<f1.getDataSize(); ++i)
    {
        ASSERT_DOUBLE_AE(f1[i], result.outSOS[i])<<"index:"<<i;
    }
}

TEST_F(Reconstruction_Test, checkAndScale) {
    MatlabReader m("/home/sun/testData/buildMatrix.mat");
    auto i  = m.read("i1");
    auto j  = m.read("j1");
    auto s = m.read("s1");
    MatlabReader m2("/home/sun/testData/checkAndScale.mat");
    auto b = m2.read("b");

    Aurora::Sparse sparse(i,j,s,709613,1196032);
    Recon::checkAndScale(sparse,b,1196032);
}

TEST_F(Reconstruction_Test, buildMatrix) {
    MatlabReader m("/home/sun/testData/buildMatrix.mat");
    auto j1 = m.read("j1");
    auto i1 = m.read("i1");
    auto s1 = m.read("s1");
    auto dims = m.read("dims");
    auto receiverList  = m.read("receiverList");
    auto potentialMap = m.read("potentialMap");
    auto senderList  = m.read("senderList"); 
    auto res  = m.read("res");
    auto result = Recon::buildMatrix(senderList, receiverList, res, dims, false, potentialMap);
    for(size_t i=0; i<i1.getDataSize(); ++i)
    {
        EXPECT_DOUBLE_AE(i1[i] - 1, result.M.getColVector()[i]);
    }

    for(size_t i=0; i<j1.getDataSize(); ++i)
    {
        EXPECT_DOUBLE_AE(j1[i]- 1, result.M.getRowVector()[i]);
    }

    for(size_t i=0; i<s1.getDataSize(); ++i)
    {
        EXPECT_DOUBLE_AE(s1[i], result.M.getValVector()[i]);
    }

}

TEST_F(Reconstruction_Test, calculateMinimalMaximalTransducerPositions) {
    auto senderList  = Aurora::Matrix::fromRawData(new double[6]{1, 2, 3, 1, 2, 4}, 3, 2);
    auto receiverList  = Aurora::Matrix::fromRawData(new double[6]{1, 8, 3, 1, 2, 1}, 3, 2);
    auto result = Recon::calculateMinimalMaximalTransducerPositions(senderList,receiverList);
    EXPECT_DOUBLE_EQ(1.0,result.getData()[0]);
    EXPECT_DOUBLE_EQ(2,result.getData()[1]);
    EXPECT_DOUBLE_EQ(1.0,result.getData()[2]);
    EXPECT_DOUBLE_EQ(1.0,result.getData()[3]);
    EXPECT_DOUBLE_EQ(8,result.getData()[4]);
    EXPECT_DOUBLE_EQ(4,result.getData()[5]); 
}

TEST_F(Reconstruction_Test, calculateResolution) {
    auto ddims   = Aurora::Matrix::fromRawData(new double[6]{-0.1296,-0.1296,0.0185,0.1296,0.1295,0.1682}, 1, 6);
    auto dims  = Aurora::Matrix::fromRawData(new double[3]{128,128,74}, 1, 3);
    auto result  = Recon::calculateResolution(ddims, dims);
    EXPECT_DOUBLE_AE(0.0020,result[0]);
    EXPECT_DOUBLE_AE(0.0020,result[1]);
    EXPECT_DOUBLE_AE(0.0021,result[2]);
}

TEST_F(Reconstruction_Test, getDimensions) {
    auto ddims = Aurora::Matrix::fromRawData(new double[6]{-0.1296,-0.1296,0.0185,0.1296,0.1295,0.1682}, 1, 6);
    double numPixelXY = 128;
    auto result  = Recon::getDimensions(numPixelXY,ddims);
    EXPECT_DOUBLE_AE(128,result[0]);
    EXPECT_DOUBLE_AE(128,result[1]);
    EXPECT_DOUBLE_AE(74,result[2]);
}

TEST_F(Reconstruction_Test, discretizePositions) {
    MatlabReader m("/home/sun/testData/discretizePositions.mat");
    auto senderList  = m.read("senderList");
    auto receiverList  = m.read("receiverList");
    auto senderListResult = m.read("senderListResult");
    auto receiverListResult  = m.read("receiverListResult");
    auto dims = m.read("dims");
    auto ddims  = m.read("ddims");
    auto res  = m.read("res");
    auto result  = Recon::discretizePositions(senderList,receiverList,Recon::transParams::numPixelXY);

    
    EXPECT_DOUBLE_AE(senderListResult.getDataSize(), result.senderCoordList.getDataSize());
    for(size_t i=0; i<senderListResult.getDataSize(); ++i)
    {
        EXPECT_DOUBLE_AE(senderListResult[i], result.senderCoordList[i]);
    }

    EXPECT_DOUBLE_AE(receiverListResult.getDataSize(), result.receiverCoordList.getDataSize());
    for(size_t i=0; i<receiverListResult.getDataSize(); ++i)
    {
        EXPECT_DOUBLE_AE(receiverListResult[i], result.receiverCoordList[i]);
    }

    EXPECT_DOUBLE_AE(dims.getDataSize(), result.dims.getDataSize());
    for(size_t i=0; i<dims.getDataSize(); ++i)
    {
        EXPECT_DOUBLE_AE(dims[i], result.dims[i]);
    }  

    EXPECT_DOUBLE_AE(ddims.getDataSize(), result.ddims.getDataSize());
    for(size_t i=0; i<ddims.getDataSize(); ++i)
    {
        EXPECT_DOUBLE_AE(ddims[i], result.ddims[i]);
    } 

    EXPECT_DOUBLE_AE(res.getDataSize(), result.res.getDataSize());
    for(size_t i=0; i<res.getDataSize(); ++i)
    {
        EXPECT_DOUBLE_AE(res[i], result.res[i]);
    }           
}

TEST_F(Reconstruction_Test, DGradient) {
    MatlabReader m("/home/krad/TestData/DGradient.mat");

    auto x = m.read("x");
    auto y = m.read("y");
    // x.forceReshape(x.getDataSize(), 1, 1);
    auto result = Recon::DGradient(x,1,2);
    for (size_t i = 0; i < result.getDataSize(); i++)
    {
        EXPECT_DOUBLE_AE(result.getData()[i],y.getData()[i])<<"index:"<<i;
    }
}

TEST_F(Reconstruction_Test, correctPath) {
    auto path  = Aurora::Matrix::fromRawData(new double[6]{1,1,10,9,1,1},2,3);
    auto startPt = Aurora::Matrix::fromRawData(new double[3]{1, .5, 1}, 1, 3);
    auto endPt = Aurora::Matrix::fromRawData(new double[3]{1, 10, 1}, 1, 3);

    Recon::correctPath(path,startPt,endPt);
    for (size_t i = 0; i < 10; i++)
    {
        EXPECT_DOUBLE_EQ(path[i],1.0);
        EXPECT_DOUBLE_EQ(path[i+10],1.0+i);
        EXPECT_DOUBLE_EQ(path[i+20],1.0);
    }
}

TEST_F(Reconstruction_Test,getPixelLengthApproximation){
    auto startPt = Aurora::Matrix::fromRawData(new double[3]{1, 1, 1}, 1, 3);
    auto endPt = Aurora::Matrix::fromRawData(new double[3]{4, 5, 6}, 1, 3);
    auto res = Aurora::Matrix::fromRawData(new double[3]{.1, .1, .1}, 1, 3);
    auto weight = Recon::getPixelLengthApproximation(startPt, endPt, res, 10);
    EXPECT_DOUBLE_AE(weight[0],.0707);
}

TEST_F(Reconstruction_Test,traceLine3D){
    auto p1 = Aurora::Matrix::fromRawData(new double[3]{1, 10, 12}, 1, 3);
    auto p2 = Aurora::Matrix::fromRawData(new double[3]{20, 2, 13}, 1, 3);
    auto discretization = Aurora::Matrix::fromRawData(new double[3]{1, 1, 1}, 1, 3);
    auto result = Recon::traceLine3D(p1, p2, discretization);
    MatlabReader m("/home/krad/TestData/traceLine3D.mat");

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

TEST_F(Reconstruction_Test,traceStraightRay){
    auto p1 = Aurora::Matrix::fromRawData(new double[3]{1, 1, 1}, 1, 3);
    auto p2 = Aurora::Matrix::fromRawData(new double[3]{5, 6, 7}, 1, 3);
    auto res = Aurora::Matrix::fromRawData(new double[3]{.1, .1, .1}, 1, 3);
    auto dmis = Aurora::Matrix::fromRawData(new double[3]{5, 10, 20}, 1, 3);
    auto result = Recon::traceStraightRay(p1, p2, res, dmis);
    EXPECT_EQ(13, result.weighting.getDataSize());
    EXPECT_DOUBLE_AE(result.weighting[0],0);
    EXPECT_DOUBLE_AE(result.weighting[1],0.1462);
    EXPECT_DOUBLE_AE(result.weighting[11],0.0292);
    EXPECT_DOUBLE_AE(result.weighting[3],0.0439);
    EXPECT_EQ(13, result.path.getDimSize(0));
    EXPECT_EQ(3, result.path.getDimSize(1));
    EXPECT_DOUBLE_AE(result.path[0],1);
    EXPECT_DOUBLE_AE(result.path[15],2);
    EXPECT_DOUBLE_AE(result.path[30],3);
    EXPECT_EQ(13, result.pathLen);
}

TEST_F(Reconstruction_Test,traceStraightRayBresenham){
    auto p1 = Aurora::Matrix::fromRawData(new double[3]{1, 1, 1}, 1, 3);
    auto p2 = Aurora::Matrix::fromRawData(new double[3]{5, 6, 7}, 1, 3);
    auto res = Aurora::Matrix::fromRawData(new double[3]{.1, .1, .1}, 1, 3);
    auto result = Recon::traceStraightRayBresenham(p1, p2, res);
    EXPECT_EQ(1, result.weighting.getDataSize());
    EXPECT_DOUBLE_AE(result.weighting[0],0.1462);
    EXPECT_EQ(6, result.path.getDimSize(0));
    EXPECT_EQ(3, result.path.getDimSize(1));
    EXPECT_DOUBLE_AE(result.path[0],1);
    EXPECT_DOUBLE_AE(result.path[15],4);
    EXPECT_DOUBLE_AE(result.path[7],2);
    EXPECT_EQ(6, result.pathLen);
}

TEST_F(Reconstruction_Test,callTval3){
    TVALOptions opt;
    opt.nonneg = false;
    opt.bent = false;
    opt.tol = 1E-10;
    opt.maxit = 50;
    opt.TVnorm = 2;
    opt.disp = false;
    opt.mu0 = 100;
    opt.mu = 100;
    opt.beta = 1;
    opt.beta0 = 1;
    MatlabReader m("/home/sun/testData/buildMatrixTest.mat");
    auto j1 = m.read("j1");
    auto i1 = m.read("i1");
    auto s1 = m.read("s1");
    Aurora::Sparse M(i1-1,j1-1,s1,734989,1196032);
    MatlabReader m2("/home/sun/testData/tval3gpu3d.mat");
    auto b = m2.read("b");
    auto dims = Aurora::Matrix::fromRawData(new double[3]{128,128,73}, 1, 3);
    auto result = Recon::callTval3(M, b, dims, 0,opt);
    auto outSOS = Recon::slownessToSOS(result, 1.498206569328594e+03) ;
    MatlabWriter w2("/home/krad/transmissionSOS111.mat");
    w2.write(outSOS, "SOS");
}