#include "reconstruction.h"
#include "Function.h"
#include "Function1D.h"
#include "Function2D.h"
#include "Function3D.h"
#include "Matrix.h"
#include <cmath>
#include <iostream>
using namespace Aurora;
namespace Recon {
    Aurora::Matrix calculateMinimalMaximalTransducerPositions(
        const Aurora::Matrix &aMSenderList, const Aurora::Matrix &aMReceiverList) {

        auto minEmitter = min(aMSenderList, Row);
        auto maxEmitter = max(aMSenderList, Row);

        auto minReceiver = min(aMReceiverList, Row);
        auto maxReceiver = max(aMReceiverList, Row);
        
        auto data1 = Aurora::malloc(minEmitter.getDataSize() * 2);
        auto data2 = Aurora::malloc(minEmitter.getDataSize() * 2);

        auto minM = Matrix::New(data1,minEmitter.getDimSize(0),2);
        auto maxM = Matrix::New(data2,minEmitter.getDimSize(0),2);
        minM($,0) = minEmitter;
        minM($,1) = minReceiver;
        maxM($,0) = maxEmitter;
        maxM($,1) = maxReceiver;
        minM = min(minM,Row);
        maxM = max(maxM,Row);
        auto data3 = Aurora::malloc(minM.getDataSize() * 2);
        auto ddims  = Matrix::New(data3,minM.getDimSize(0),2);
        ddims($,0) = minM;
        ddims($,1) = maxM;
        ddims.forceReshape(1, ddims.getDataSize(), 1);
        return  ddims;
    }
    
    Aurora::Matrix calculateResolution(const Aurora::Matrix &aVDdims, const Aurora::Matrix &aVDims)
    {
        auto numDim = aVDims.getDataSize();
        auto res = (aVDdims.block(1,numDim,aVDdims.getDataSize()-1) - aVDdims.block(1,0,numDim-1))/ (aVDims - 1);

        if(numDim == 3 && aVDims[2] == 1){
            res[2] = res[0];
        }
        return res;
    }
    
    Aurora::Matrix getDimensions(double aNumPixelXY, const Aurora::Matrix& aVDdims)
    {
        int numDim = aVDdims.getDataSize()/2;

        if (!(numDim == 2 || numDim == 3)){
           std::cerr<<"Inputs does not match requirements."<<std::endl;
           return Aurora::Matrix();
        }

        auto dims = ones(1,numDim);

        dims[0] = aNumPixelXY;
        dims[1] = aNumPixelXY; 

        if (numDim == 3)
        {
            dims[2] = std::ceil((aVDdims[5]-aVDdims[2])/((aVDdims[3]-aVDdims[0])/dims[0]));
            //BUG:发现源代码可能存在无限值，加了std::isfinite验证
            if(dims[2] == 0 || !std::isfinite(dims[2]))dims[2] = 1;
        }
        return dims;
    }
    
    void slownessToSOS(Aurora::Matrix & aVF1, double aSOS_IN_WATER)
    {
        aVF1 = 1 / ((aVF1 + 1) / aSOS_IN_WATER);
    }
    
    DiscretizePositionValues discretizePositions(Aurora::Matrix &aVSenderCoordList, Aurora::Matrix &aVReceiverCoordList, double aNumPixelXY)
    {
        DiscretizePositionValues result;
        result.ddims = calculateMinimalMaximalTransducerPositions(aVSenderCoordList, aVReceiverCoordList);
        result.dims = getDimensions(aNumPixelXY, result.ddims);
        result.res = calculateResolution(result.ddims, result.dims);
        int numDim = aVSenderCoordList.getDimSize(0);
        auto tempDdims = result.ddims.block(0,0,numDim-1);
        auto tempTransRes = repmat(transpose(result.res),1,2);
        auto tempDivRes = repmat(transpose(tempDdims / result.res),1,2);
        aVSenderCoordList = (aVSenderCoordList / tempTransRes) - tempDivRes + 1;
        aVReceiverCoordList = (aVReceiverCoordList / tempTransRes) - tempDivRes + 1;
        result.receiverCoordList = aVReceiverCoordList;
        result.senderCoordList = aVSenderCoordList;
        return result;
    }
} // namespace Recon