#include "getMeasurementMetaData.h"

#include <algorithm>
#include <mkl.h>

#include "Function.h"
#include "Function2D.h"

#include "Parser.h"
#include "Data/MetaData.h"
#include "Data/MovementData.h"
#include "Data/TemperatureData.h"
#include "Data/CEMeasuredData.h"
#include "Data/CEData.h"

#include "ceMatchedFilterHandling.h"
#include "ShotList/ShotList.h"
#include "config/config.h"


using namespace Recon;
using namespace Aurora;

Matrix Recon::getAvailableMotorPositions(Parser* aParser)
{
    std::vector<float> mpData;
    if(aParser->hasCEMeasured())
    {
        mpData.push_back(0);
    }
    int motorPosition = aParser->getMetaData().getAperturePositionNumber();
    for(int i=1; i<=motorPosition; ++i)
    {
        mpData.push_back(i);
    }

    return Matrix::copyFromRawData(mpData.data(), mpData.size());
}

//已验证 完全正确
MeasurementInfo Recon::loadMeasurementInfos(Parser* aParser)
{
    MeasurementInfo result;
    MetaData metaData = aParser->getMetaData();
    result.numberSamples = metaData.getSampleNumber();
    result.Bandpassundersampling = false;
    result.sampleRate = metaData.getSamplerate();
    result.ascanDataType = metaData.getDataType();
    result.Hardware = "USCT3dv3";
    result.HardwareVersion = "3.0";
    result.EOffset = 0;
    result.Wavelength = 3;
    result.expectedAScanLength = result.numberSamples;
    result.rootMeasUniqueID = metaData.getMeasurementID();
    result.dacDelay = metaData.getDacDelay();
    result.filterByPass = metaData.getFilterByPass();
    if(result.Bandpassundersampling)
    {
        result.expectedAScanLength = reflectParams::aScanReconstructionFrequency / result.sampleRate * result.expectedAScanLength;
    }
    return result;
}

//已验证 完全正确
TransFormInfo Recon::getTransformationMatrix(Parser* aParser, const Matrix& aMotorPosList)
{
    TransFormInfo result;
    bool movementRealAvailable = true;
    Matrix transformationMatrixList;
    MovementsListRealPointer listRealData = aParser->getMovementData().getMovementsListReal();
    int columns = 2;//一个角度一个高度
    unsigned long long length = listRealData.getLength();
    int rows = length / columns;
    float* data = new float[length];
    std::copy(listRealData.get(), listRealData.get() + length, data);
    std::vector<int> info = {rows, columns, 1};
    Matrix movementsListReal(std::shared_ptr<float>(data, std::default_delete<float[]>()), info);
    float* motorPosListAvailableData = Aurora::malloc(rows);
    for(int i=0; i<rows; ++i)
    {
        motorPosListAvailableData[i] = i + 1;
    }
    Matrix motorPosListAvailable = Matrix::New(motorPosListAvailableData, rows);
    Matrix motorPosList = intersect(motorPosListAvailable, aMotorPosList);
    result.motorPos = motorPosList;

    if(movementsListReal.isNan())
    {
        movementRealAvailable = false;
    }
    //movementRealAvailable=false;
    if(movementRealAvailable)
    {
        int maxMotorPosNum = motorPosList.getDimSize(0);
        float* transformationMatrixListData = Aurora::malloc(16*maxMotorPosNum);
        float zero = 0.0;
        cblas_scopy(16*maxMotorPosNum,&zero,0,transformationMatrixListData,1);
        for(int i=0; i<maxMotorPosNum; ++i)
        {
            float rotation = movementsListReal.getData()[i];
            float distance = movementsListReal.getData()[i + aParser->getMetaData().getAperturePositionNumber()];
            transformationMatrixListData[16*i + 0] = cos(rotation*PI/180);
            transformationMatrixListData[16*i + 1] = sin(rotation*PI/180);
            transformationMatrixListData[16*i + 4] = -sin(rotation*PI/180);
            transformationMatrixListData[16*i + 5] = cos(rotation*PI/180);
            transformationMatrixListData[16*i + 10] = 1;
            transformationMatrixListData[16*i + 14] = distance;
            transformationMatrixListData[16*i + 15] = 1;
        }
        transformationMatrixList = Matrix::New(transformationMatrixListData, 4, 4,maxMotorPosNum);
    }
    else
    {
        auto rotationMatrix = aParser->getMovementData().getRotationMatrix();
        int maxMotorPosNum = motorPosList.getDimSize(0);
        float* transformationMatrixListData = new float[16*maxMotorPosNum];
        for(size_t i=0; i<maxMotorPosNum; ++i)
        {
            std::copy(rotationMatrix.at(i).get(), rotationMatrix.at(i).get()+16, transformationMatrixListData + i*16);
        }
        transformationMatrixList = Matrix::fromRawData(transformationMatrixListData, 4, 4, maxMotorPosNum);
    }
    result.rotationMatrix = transformationMatrixList;
    return result;
}

Matrix Recon::temperatureToSoundSpeed(const Matrix& aTemperature, const std::string& aMethod)
{
    Matrix result;
    if (aMethod == "marczak")
    {
        float* kData = new float[6] {2.78786e-9, -1.398845e-6, 3.287156e-4, -5.799136e-2, 5.038813, 1.402385e3};
        Matrix k = Matrix::fromRawData(kData, 6);
        result = polyval(k, aTemperature);
    }
    else if(aMethod == "mader")
    {
        float* kData = new float[6] {3.14643e-9, -1.478e-6, 0.000334199, -0.0580852, 5.03711, 1402.39};
        Matrix k = Matrix::fromRawData(kData, 6);
        result = polyval(k, aTemperature);
    }
    else if(aMethod == "jan")
    {
        float speedData = 1557 - 0.0245 * pow((74 - aTemperature.getData()[0]), 2);
        result = Matrix::fromRawData(new float[1] {speedData}, 1);
    }
    return result;
}

//已验证，完全正确
TempInfo Recon::getTemperatureInfo(Parser* aParser, float aNumTas)
{
    TempInfo result;
    //jumoTemp
    JumoTemperaturePointer jumoTemp1 = aParser->getTemperatureData().getJumoTemperature1();
    JumoTemperaturePointer jumoTemp2 = aParser->getTemperatureData().getJumoTemperature1();
    JumoTemperaturePointer jumoTemp3 = aParser->getTemperatureData().getJumoTemperature1();
    JumoTemperaturePointer jumoTemp4 = aParser->getTemperatureData().getJumoTemperature1();
    int jumoTempNum = jumoTemp1.getLength();
    if(jumoTemp2.getLength() < jumoTempNum)
    {
        jumoTempNum = jumoTemp2.getLength();
    }

    if(jumoTemp3.getLength() < jumoTempNum)
    {
        jumoTempNum = jumoTemp3.getLength();
    }

    if(jumoTemp4.getLength() < jumoTempNum)
    {
        jumoTempNum = jumoTemp4.getLength();
    }

    float* jumoTempData = Aurora::malloc(jumoTempNum * 4);
    for(int i=0; i<jumoTempNum; ++i)
    {
        jumoTempData[4*i] = jumoTemp1.get()[i];
        jumoTempData[4*i + 1] = jumoTemp2.get()[i];
        jumoTempData[4*i + 2] = jumoTemp3.get()[i];
        jumoTempData[4*i + 3] = jumoTemp4.get()[i];
    }
    result.jumoTemp = Aurora::Matrix::New(jumoTempData, 4, jumoTempNum);//输出
    bool tasTempUsed = false;

    //Tas Temperature
    if(reconParams::useTASTempComp)
    {
        //todo TemperatureModel4D字段不存在
        //  try
        // % load temp from files.tempTASComp (standard file, including the preprocessed/corrected temperature data)
        // % and extract infos, TemperatureModel4D and TASTemperature
        // TASTemp = loadTASTemperaturesProcessed(path, files.tempTASComp);

        // % extract TemperatureModel4D
        // if isfield(TASTemp, 'TemperatureModel4D')
        //     temp.TemperatureModel4D = TASTemp.TemperatureModel4D;
        // end

        // % extract TASTemperatures
        // temp.TASTemperature = TASTemp.TASTemperature;
        // catch
        //     tasTempCompUsed = 0;
        // end
    }

    if (!tasTempUsed)
    {
        size_t tasTempLength = aParser->getTemperatureData().getTasTemperature()[0].getLength();
        float* fromTasTempData = aParser->getTemperatureData().getTasTemperature()[0].get();
        float* tasTempData = new float[tasTempLength];
        std::copy(fromTasTempData, fromTasTempData+ tasTempLength, tasTempData);
        Matrix tasTemp = Matrix::fromRawData(tasTempData, 2, tasTempLength / 2);

        if (!reconParams::correctTASTemp || result.jumoTemp.isNan())
        {
            result.tasTemperature = tasTemp;
        }
        else
        {
            //todo JumoTemp都没有值
            // temp.TASTemperature(1, :, :) = correctTASTemperatures(temp2, temp.jumoTemp);

            //         catch ME

            //     % if tas temperatures not there
            //     % take temperatures from calibrated sensors
            //     % and use them for all TAS the same

            //     writeReconstructionLog(sprintf('%s: %s TAS temperatures approximated from calibrated reference sensors.', ME.identifier, ME.message), 3)

            //     if(~jumoAllNaN)
            //         temp.TASTemperature = zeros(2, numTAS, size(temp.jumoTemp, 2));
            //         temp.TASTemperature(1, :, :) = repmat(mean(temp.jumoTemp), size(temp.TASTemperature, 2), 1);
            //         temp.TASTemperature(2, :, :) = repmat([1:size(temp.TASTemperature, 2)]', 1, size(temp.TASTemperature, 3));
            //     else % worst case: no temperature data at all! cannot continue!
            //         error([mfilename ':noTemperatureAvailable'],'No TAS and no Jumo temperature available! Cannot reconstruct!');
            //     end

            // end
        }
    }

    if (result.jumoTemp.isNan())
    {
        result.expectedTemp = mean(result.tasTemperature(0,$,$).toMatrix(),FunctionDirection::Column,false);
        result.jumoTemp = result.expectedTemp;
    }
    else
    {
        result.expectedTemp = mean(result.jumoTemp,FunctionDirection::All,false);
    }
    result.expectedSOSWater = temperatureToSoundSpeed(result.expectedTemp , "marczak");

    return result;
}

//已验证 完全正确
CEInfo Recon::getCEInfo(Parser* aParser, const MeasurementInfo aInfo)
{
    CEInfo result;
    result.measuredCEUsed = reconParams::useCEMeasured;
    Matrix ce;
    auto ceMeasuredPointer = aParser->getAllCEMeasuredData();
    if (ceMeasuredPointer.isNull())
    {
        result.measuredCEAvailable = false;
        result.measuredCEUsed = false;
    }
    else
    {
        result.measuredCEAvailable = true;
        size_t ceMeasuredLength = ceMeasuredPointer.getAscanDataLength();
        short* ceMeasuredData = ceMeasuredPointer.get();
        if(aParser->getMetaData().getDataType() == "float16")
        {
            ce = convertfp16tofloat(ceMeasuredPointer.get(), ceMeasuredLength, ceMeasuredPointer.getSize());
        }
        else
        {
            float* ceData = new float[ceMeasuredLength * ceMeasuredPointer.getSize()];
            std::copy(ceMeasuredData, ceMeasuredData + ceMeasuredLength * ceMeasuredPointer.getSize(), ceData);
            ce = Matrix::fromRawData(ceData, ceMeasuredLength, ceMeasuredPointer.getSize());
        }
        if(aInfo.Bandpassundersampling)
        {
            // todo 2代变量
            // ceOut(measInfo.NumberSamples, :) = 0; % expand to 1000 could lead to problems in future????
            // ceOut = reconstructBandpasssubsampling(ceOut, reconstructionFreq, ce.CE_SF);
        }
        if (ce.getDimSize(0) < aInfo.numberSamples)
        {
            // todo 现有逻辑没有这种可能
            // indInsert = size(ceOut, 1) + 1:measInfo.NumberSamples;
            // ceOut(indInsert, :) = repmat(ceOut(size(ceOut, 1), :), length(indInsert), 1); % expand to 1000 could lead to problems in future????
        }
        if (reflectParams::removeDCOffset)
        {
            ce = ce - repmat(mean(ce),ce.getDimSize(0),1);
        }
        result.ce = ce;
        result.ceOffSet = aParser->getCEData().getCEOffset();
        result.ce_sf = aParser->getCEData().getCE_SF();
        if (aInfo.Hardware == "USCT3dv3")
        {
            size_t size = aParser->getMetaData().getTasIndices().getLength();
            uint8_t* fromData = aParser->getMetaData().getTasIndices().get();
            float* tasIndicesData = new float[size];
            std::copy(fromData, fromData + size, tasIndicesData);
            result.tasIndices = Matrix::fromRawData(tasIndicesData, size);

            size = aParser->getMetaData().getReceiverIndices().getLength();
            fromData = aParser->getMetaData().getReceiverIndices().get();
            float* receiverIndicesData = new float[size];
            std::copy(fromData, fromData + size, receiverIndicesData);
            result.receiverIndices = Matrix::fromRawData(receiverIndicesData, size);
        }
    }


    result.ceRefOffSet = aParser->getCEData().getCEOffset();
    result.ceRef_sf = aParser->getCEData().getCE_SF();
    auto ceRefFromData = aParser->getCEData().getCE();
    if (ceRefFromData.isNull())
    {
        result.ceAvailable = false;
    }
    else
    {
        result.ceAvailable = true;
        size_t ceRefSize = ceRefFromData.getLength();
        float* ceRefData = new float[ceRefSize];
        std::copy(ceRefFromData.get(), ceRefFromData.get() + ceRefSize, ceRefData);
        Matrix ceRefCE = Matrix::fromRawData(ceRefData, ceRefSize);
        result.ceRef = preprocessCE(ceRefCE, result.ceRef_sf, reflectParams::aScanReconstructionFrequency, aInfo.expectedAScanLength);
    }
    result.offsetFilterEnabled = reconParams::offsetFilterEnabled;
    result.offsetFilterDisabled = reconParams::offsetFilterDisabled;

    return result;
}