#include "preprocessTransmissionReconstructionForReflection.h"
#include "Function1D.h"
#include "Function3D.h"
#include "Function2D.h"
#include "Matrix.h"
#include "reflectionReconstruction/preprocessData/resampleTransmissionVolume.h"
#include "src/config/config.h"

#include <algorithm>
#include <iostream>
#include <cstddef>

using namespace Aurora;
using namespace Recon;

PreprocessReflectionData Recon::preprocessTransmissionReconstructionForReflection(const Aurora::Matrix& aSOSMap, const Aurora::Matrix& aATTMap,
                                                         const Aurora::Matrix& aDdims, const GeometryInfo& aGeometryInfo,
                                                         const TempInfo& aTemp)
{
    PreprocessReflectionData result;
    int soundSpeedCorrection = reflectParams::soundSpeedCorrection;
    int attenuationCorrection = reflectParams::attenuationCorrection;
    Matrix minPos;
    Matrix maxPos;
    if(!aDdims.isNull())
    {
        minPos = Matrix::fromRawData(new double[3], 1, 3);
        size_t maxPosSize = aDdims.getDataSize() - 3;
        maxPos = Matrix::fromRawData(new double[maxPosSize], 1, maxPosSize);
        std::copy(aDdims.getData(), aDdims.getData() + 3, minPos.getData());
        std::copy(aDdims.getData() + 3, aDdims.getData() + 3 + maxPosSize, maxPos.getData());
    }

    if(soundSpeedCorrection == 1)
    {
        auto sosResampleVolume = resampleTransmissionVolume(aSOSMap, maxPos, minPos, aGeometryInfo);
        result.transRecos.deltaTransMap = sosResampleVolume.deltaTransMap;
        result.transRecos.beginTransMap = sosResampleVolume.beginTransMap;
        // TemperatureModel4D暂无 todo
        // try % use temp model 4D
        // speedmapx=transRecos.begin_TransMap(1):transRecos.delta_TransMap:(size(transRecos.SpeedMap3D,1)-1)*transRecos.delta_TransMap + transRecos.begin_TransMap(1);
        // speedmapy=transRecos.begin_TransMap(2):transRecos.delta_TransMap:(size(transRecos.SpeedMap3D,2)-1)*transRecos.delta_TransMap + transRecos.begin_TransMap(2);
        // speedmapz=transRecos.begin_TransMap(3):transRecos.delta_TransMap:(size(transRecos.SpeedMap3D,3)-1)*transRecos.delta_TransMap + transRecos.begin_TransMap(3);
        // tidx=abs(transRecos.SpeedMap3D(:))<1; % voxels to fill
        // [ix,iy,iz]=ind2sub(size(transRecos.SpeedMap3D),find(tidx));
        
        // %fill with model
        // if(~isempty(ix) && ~isempty(iy) && ~isempty(iz))
        //     transRecos.SpeedMap3D(tidx) = temperatureToSoundSpeed(polyvaln(temp.TemperatureModel4D.p,[speedmapx(ix)',speedmapy(iy)', speedmapz(iz)', zeros(size(ix))])); %surf(xg,yg,zg,reshape(tg,size(xg)))
        // end
        // catch
        Matrix sppedMap3d = sosResampleVolume.transMap;
        for(size_t i=0; i<sppedMap3d.getDataSize(); ++i)
        {
            if(std::abs(sppedMap3d[i]) < 1)
            {
                sppedMap3d[i] = aTemp.expectedSOSWater[0];
            }
        }
        result.transRecos.speedMap3D = sppedMap3d;
        //end
    }

    if( attenuationCorrection == 1)
    {
        Matrix attenuationMap3D = aATTMap.deepCopy();
        for(size_t i=0; i<attenuationMap3D.getDataSize(); ++i)
        {
            if(attenuationMap3D[i] < 0)
            {
                attenuationMap3D[i] = 0;
            }
        }
        attenuationMap3D = attenuationMap3D*20/std::log(10);
        attenuationMap3D = attenuationMap3D*100;

       
        auto attResampleVolume = resampleTransmissionVolume(attenuationMap3D, maxPos, minPos, aGeometryInfo);
        result.transRecos.deltaTransMap = attResampleVolume.deltaTransMap;
        result.transRecos.beginTransMap = attResampleVolume.beginTransMap;
        attenuationMap3D = attResampleVolume.transMap;
        if(result.transRecos.speedMap3D.isNull())
        {
            result.transRecos.speedMap3D = zeros(attenuationMap3D.getDimSize(0), attenuationMap3D.getDimSize(1), attenuationMap3D.getDimSize(2));
            soundSpeedCorrection = 1;
        }
        result.transRecos.attenuationMap3D = attenuationMap3D;
    }
    else if(soundSpeedCorrection == 1 && !result.transRecos.speedMap3D.isNull())
    {
        Matrix speedMap3D = result.transRecos.speedMap3D;
        result.transRecos.attenuationMap3D = zeros(speedMap3D.getDimSize(0), speedMap3D.getDimSize(1), speedMap3D.getDimSize(2));
        attenuationCorrection = 1;
    }

    if(soundSpeedCorrection == 0 && attenuationCorrection == 0)
    {
        result.saftMode = 2;
        double deltaTransMap = reflectParams::resolutionTransmMap;
    
        Matrix minTransdPos = min(aGeometryInfo.minEmitter, aGeometryInfo.minReceiver);
        Matrix maxTransdPos = max(aGeometryInfo.maxEmitter, aGeometryInfo.maxReceiver);
        Matrix begin_TransMap = transpose(min(transpose(reflectParams::imageStartpoint), minTransdPos) - 3 * reflectParams::resolutionTransmMap);
        Matrix end_SpeedMap = max(transpose(reflectParams::imageEndpoint),maxTransdPos) + 3 * reflectParams::resolutionTransmMap;
        end_SpeedMap[2] = end_SpeedMap[2] + 0.025;
        
        Matrix x = createVectorMatrix(begin_TransMap[0], deltaTransMap, end_SpeedMap[0]) + 0.5 * deltaTransMap;
        Matrix y = createVectorMatrix(begin_TransMap[1], deltaTransMap, end_SpeedMap[1]) + 0.5 * deltaTransMap;
        Matrix z = createVectorMatrix(begin_TransMap[2], deltaTransMap, end_SpeedMap[2]) + 0.5 * deltaTransMap;
        
        // TemperatureModel4D暂无 todo
        // try 
        //     transRecos.SpeedMap3D = reshape(temperatureToSoundSpeed(polyvaln(temp.TemperatureModel4D.p,[XNEW(:),YNEW(:), ZNEW(:), zeros(size(XNEW(:)))])),size(XNEW));
        // catch

        result.transRecos.speedMap3D = zeros(x.getDimSize(1), y.getDimSize(1), z.getDimSize(1)) + aTemp.expectedSOSWater;
    
        
        result.transRecos.beginTransMap = begin_TransMap;
        result.transRecos.deltaTransMap = deltaTransMap;
        result.transRecos.attenuationMap3D = zeros(result.transRecos.speedMap3D.getDimSize(0), result.transRecos.speedMap3D.getDimSize(1), result.transRecos.speedMap3D.getDimSize(2));
        soundSpeedCorrection = 1;   
    } 
    else if (soundSpeedCorrection > 0 && attenuationCorrection == 0)
    {
        result.saftMode = 1;
        result.transRecos.attenuationMap3D = zeros(result.transRecos.speedMap3D.getDimSize(0), result.transRecos.speedMap3D.getDimSize(1), result.transRecos.speedMap3D.getDimSize(2));
    }
    else if (soundSpeedCorrection > 0 && attenuationCorrection > 0)
    {
        if (result.transRecos.attenuationMap3D.getDimSize(0) != result.transRecos.speedMap3D.getDimSize(0) ||
            result.transRecos.attenuationMap3D.getDimSize(1) != result.transRecos.speedMap3D.getDimSize(1) ||
            result.transRecos.attenuationMap3D.getDimSize(2) != result.transRecos.speedMap3D.getDimSize(2))
        {
            std::cout<<"Size of SpeedMap3D ~= Size of AttenuationMap3D."<<std::endl;
        } 
        result.saftMode = 2;
    }
    else if (soundSpeedCorrection == 0 && attenuationCorrection > 0)
    {
        std::cout<<"Only attenuation correction not allowed."<<std::endl;
    }

    result.soundSpeedCorrection = soundSpeedCorrection;
    result.attenuationCorrection = attenuationCorrection;

    return result;
}