#include "getMeasurementMetaData.h"
#include "Matrix.h"
#include "ceMatchedFilterHandling.h"

#include "ShotList/ShotList.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 "Function.h"
#include "Function1D.h"
#include "Function2D.h"
#include "AuroraDefs.h"
#include "mkl.h"

#include "../config/config.h"
#include <algorithm>

using namespace Recon;
using namespace Aurora;

Matrix Recon::getAvailableMotorPositions(Parser* aParser)
{
    std::vector<double> mpData;
    if(aParser->getShotList()->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();
    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;
    double* data = new double[length];
    std::copy(listRealData.get(), listRealData.get() + length, data);
    std::vector<int> info = {rows, columns, 1};
    Matrix movementsListReal(std::shared_ptr<double>(data, std::default_delete<double[]>()), info);
    double* 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;
    }
    if(movementRealAvailable)
    {
        int maxMotorPosNum = motorPosList.getDimSize(0);
        double* transformationMatrixListData = Aurora::malloc(16*maxMotorPosNum);
        double zero = 0.0;
        cblas_dcopy(16*maxMotorPosNum,&zero,0,transformationMatrixListData,1);
        for(int i=0; i<maxMotorPosNum; ++i)
        {
            double rotation = movementsListReal.getData()[i];
            double distance = movementsListReal.getData()[i + 2];
            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();
        size_t maxMotorPosNum = rotationMatrix.size();
        double* transformationMatrixListData = new double[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;
}

//已验证，完全正确
TempInfo Recon::getTemperatureInfo(Parser* aParser, double 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();
    }

    double* 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();
        double* tasTempData = new double[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);
    }

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

    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
        {
            double* ceData = new double[ceMeasuredLength];
            std::copy(ceMeasuredData, ceMeasuredData + ceMeasuredLength, 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 = aInfo.sampleRate;
        if (aInfo.Hardware == "USCT3dv3")
        {
            size_t size = aParser->getMetaData().getTasIndices().getLength();
            uint8_t* fromData = aParser->getMetaData().getTasIndices().get();
            double* tasIndicesData = new double[size];
            std::copy(fromData, fromData + size, tasIndicesData);
            result.tasIndices = Matrix::fromRawData(tasIndicesData, size);

            size = aParser->getMetaData().getReceiverIndices().getLength();
            fromData = aParser->getMetaData().getReceiverIndices().get();
            double* receiverIndicesData = new double[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();
        double* ceRefData = new double[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;
}