#include "getMeasurementMetaData.h"

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

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

#include "Matrix.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"
#include "temperatureCalculation/estimateSOSWater.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();
    result.ceMeasuredDacDelay = metaData.getCEMeasuredDacDelay();
    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, const CEInfo& aCEInfo)
{
    TempInfo result;
    result.expectedSOSWater = estimateSOSWater(aParser,aCEInfo);
    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;
    result.offsetTransducer = reconParams::offsetTransducer;
    return result;
}