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

#include "Function1D.h"
#include "Function3D.h"
#include "MatlabReader.h"
#include "MatlabWriter.h"
#include "Matrix.h"
#include "Sparse.h"
#include "common/getMeasurementMetaData.h"
#include "config/config.h"
#include "reflectionReconstruction/preprocessData/preprocessAScanBlockForReflection.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/preprocessTransmissionReconstructionForReflection.h"
#include "reflectionReconstruction/preprocessData/precalcImageParameters.h"
#include "reflectionReconstruction/reconstructionSAFT/reconstructionSAFT.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.925316499483377e-04;
    Recon::reflectParams::optPulseFactor = 48;
    auto result = Recon::determineOptimalPulse(1.0000e-07,4096);
    EXPECT_EQ(4096, result.getDataSize());
    MatlabReader m2("/home/krad/TestData/sincPeakft.mat");
    auto f1 = m2.read("sincPeak_ft");
    for(size_t i=0; i<f1.getDataSize(); ++i)
    {
        EXPECT_DOUBLE_AE(f1[i], 0)<<"index:"<<i;
    }
}

TEST_F(Reconstruction_Test, reconstructionSAFT) {

     Recon::initalizeConfig("");
     MatlabReader m("/home/sun/testData/recontructSAFT.mat");
     auto Ascans = m.read("AScans");
     auto AttenuationMap3D = m.read("AttenuationMap3D");
     auto begin_TransMap = m.read("begin_TransMap");
     auto delta_TransMap = m.read("delta_TransMap");
     auto EnvOut = m.read("Env");
     auto mpIndex = m.read("mpIndex");
     auto receiverPos = m.read("receiverPos");
     auto SAFT_mode = m.read("SAFT_mode");
     auto senderPos = m.read("senderPos");
     auto SpeedMap3D = m.read("SpeedMap3D");
     Recon::TransRecos transRecos;
     transRecos.speedMap3D = SpeedMap3D;
     transRecos.attenuationMap3D = AttenuationMap3D;
     transRecos.beginTransMap = begin_TransMap;
     transRecos.deltaTransMap = delta_TransMap[0];
     MatlabReader m1("/home/sun/testData/resampleTransmissionVolume.mat");
     auto maxEmitter = m1.read("maxEmitter");
     auto maxReceiver = m1.read("maxReceiver");
     auto minEmitter = m1.read("minEmitter");
     auto minReceiver = m1.read("minReceiver");
     Recon::GeometryInfo geom;
     geom.maxEmitter = maxEmitter;
     geom.minEmitter = minEmitter;
     geom.maxReceiver = maxReceiver;
     geom.minReceiver = minReceiver;
     geom.minSize = m1.read("minSize");
     geom.maxSize = m1.read("maxSize");
     precalcImageParameters(geom);
     Aurora::Matrix Env = Aurora::zeros((int)Recon::reflectParams::imageXYZ[0],(int)Recon::reflectParams::imageXYZ[1],(int)Recon::reflectParams::imageXYZ[2]);
     auto result = Recon::recontructSAFT(Ascans, senderPos, receiverPos, mpIndex, 2, transRecos, Env);
    for(size_t i=0; i<result.getDataSize(); ++i)
    {
        EXPECT_DOUBLE_AE(result[i], EnvOut[i])<<"index:"<<i;
    }

}

TEST_F(Reconstruction_Test, preprocessAScanBlockForReflection) {
    Recon::initalizeConfig("");
    MatlabReader m2("/home/krad/TestData/preprocessRefC.mat");
    auto blockedAScans = m2.read("blockedAScans");
    auto blockedMP = m2.read("blockedMP");
    auto blockedSL = m2.read("blockedSL");
    auto blockedSN = m2.read("blockedSN");
    auto blockedRL = m2.read("blockedRL");
    auto blockedRN = m2.read("blockedRN");
    auto senderPositionBlock = m2.read("blockedSenderPosition");
    auto receiverPositionBlock = m2.read("blockedReceiverPosition");
    auto channelBlock = m2.read("blockedChannels");
    auto gainBlock = m2.read("blockedGain");
    Recon::MeasurementInfo info;
    info.expectedAScanLength= 4096;
    info.Wavelength=3;
    Recon::PreComputes preComputes;
    preComputes.measuredCEUsed = true;
    preComputes.offset = 5.2000e-07;
    preComputes.timeInterval = 1.0000e-07;
    preComputes.matchedFilter =  m2.read("matchedFilter");
    MatlabReader m("/home/krad/TestData/sincPeakft.mat");
    preComputes.sincPeak_ft = m.read("sincPeak_ft");
    auto result = Recon::preprocessAScanBlockForReflection(blockedAScans, blockedMP, blockedSL, blockedSN, blockedRL,
     blockedRN,senderPositionBlock, receiverPositionBlock, gainBlock, channelBlock, info, preComputes);

    size_t size = result.AscanBlock.getDataSize();
    // for(size_t i=0; i<result.AscanBlock.getDataSize(); ++i)
    // {
    //     ASSERT_DOUBLE_AE(f1[i], result[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.setMatrix(outSOS, "SOS");
    w2.write();
}