#include <algorithm>
#include <cstddef>
#include <iostream>
#include "Function.h"
#include "Function2D.h"
#include "Function1D.h"
//必须在Eigen之前
#include "AuroraDefs.h"
#include "Function3D.h"
#include "Matrix.h"

#include <Eigen/Core>
#include <Eigen/Eigen>
#include <Eigen/Dense>
#include <iterator>
#include <utility>

using namespace Aurora;

double Aurora::immse(const Aurora::Matrix &aImageA, const Aurora::Matrix &aImageB) {
    if (aImageA.getDims()!=2|| aImageB.getDims()!=2){
        std::cerr<<"Fail!immse args must all 2d matrix!";
        return 0.0;
    }
    if (!aImageB.compareShape(aImageA)){
        std::cerr<<"Fail!immse args must be same shape!";
        return 0.0;
    }
    if (aImageA.getValueType()!=Normal || aImageB.getValueType() != Normal) {
        std::cerr << "Fail!immse args must be normal value type!";
        return 0.0;
    }
    int size = aImageA.getDataSize();
    auto temp = malloc(size);
    vdSub(size, aImageA.getData(), aImageB.getData(), temp);
    vdSqr(size, temp, temp);
    double result = cblas_dasum(size, temp, 1) / (double) size;
    free(temp);
    return result;
}

Aurora::Matrix Aurora::inv(const Aurora::Matrix &aMatrix) {
    if (aMatrix.getDims() != 2) {
        std::cerr << "Fail!inv args must be 2d matrix!";
        return aMatrix;
    }
    if (aMatrix.getDimSize(0) != aMatrix.getDimSize(1)) {
        std::cerr << "Fail!inv args must be square matrix!";
        return aMatrix;
    }
    if (aMatrix.getValueType() != Normal) {
        std::cerr << "Fail!inv args must be normal value type!";
        return aMatrix;
    }
    int size = aMatrix.getDataSize();
    int *ipiv = new int[aMatrix.getDimSize(0)];
    auto result = malloc(size);
    cblas_dcopy(size,aMatrix.getData(), 1,result, 1);
    LAPACKE_dgetrf(LAPACK_ROW_MAJOR, aMatrix.getDimSize(0), aMatrix.getDimSize(0), result, aMatrix.getDimSize(0), ipiv);
    LAPACKE_dgetri(LAPACK_ROW_MAJOR, aMatrix.getDimSize(0), result, aMatrix.getDimSize(0), ipiv);
    delete[] ipiv;
    return Matrix::New(result,aMatrix);
}

Aurora::Matrix Aurora::inv(Aurora::Matrix&& aMatrix) {
    if (aMatrix.getDims() != 2) {
        std::cerr << "Fail!inv args must be 2d matrix!";
        return aMatrix;
    }
    if (aMatrix.getDimSize(0) != aMatrix.getDimSize(1)) {
        std::cerr << "Fail!inv args must be square matrix!";
        return aMatrix;
    }
    if (aMatrix.getValueType() != Normal) {
        std::cerr << "Fail!inv args must be normal value type!";
        return aMatrix;
    }
    int *ipiv = new int[aMatrix.getDimSize(0)];
    LAPACKE_dgetrf(LAPACK_ROW_MAJOR, aMatrix.getDimSize(0), aMatrix.getDimSize(0), aMatrix.getData(), aMatrix.getDimSize(0), ipiv);
    LAPACKE_dgetri(LAPACK_ROW_MAJOR, aMatrix.getDimSize(0), aMatrix.getData(), aMatrix.getDimSize(0), ipiv);
    delete[] ipiv;
    return aMatrix;
}

Matrix Aurora::interp2(const Matrix& aX, const Matrix& aY, const Matrix& aV, const Matrix& aX1, const Matrix& aY1, InterpnMethod aMethod)
{
    if (aV.getDims() != 2)
    {
        return Matrix();
    }

    if (aX1.getDimSize(0) != aY1.getDimSize(0))
    {
        return Matrix();
    }

    int columnNum = aV.getDimSize(1);
    int rowNum = aV.getDimSize(0);

    if(aX.getDimSize(0) != columnNum || aY.getDimSize(0) != rowNum)
    {
      return Matrix();
    }

    int nx1 = aX1.getDimSize(0);
    std::shared_ptr<double> resultData = std::shared_ptr<double>(Aurora::malloc(nx1), Aurora::free);
    for (int i = 0; i < nx1; ++i)
    {
        std::shared_ptr<double> xResultData = std::shared_ptr<double>(Aurora::malloc(columnNum), Aurora::free);
        for(int j =0; j < columnNum; ++j)
        {
            xResultData.get()[j] = interp1(aY,aV($,j).toMatrix(),aY1(i).toMatrix(),aMethod).getData()[0];
        }
        Matrix xResult(xResultData,std::vector<int>{columnNum});
        resultData.get()[i] = interp1(aX,xResult,aX1(i).toMatrix(),aMethod).getData()[0];
    }
    return Matrix(resultData,std::vector<int>{nx1});
}

Matrix Aurora::interpn(const Matrix& aX, const Matrix& aY, const Matrix& aV, const Matrix& aX1, const Matrix& aY1, InterpnMethod aMethod)
{
    return Aurora::interp2(aY,aX,aV,aY1,aX1,aMethod);
}

Matrix Aurora::std(const Matrix &aMatrix) {
    if (aMatrix.getDimSize(2) > 1 || aMatrix.isComplex()) {
        std::cerr
                << (aMatrix.getDimSize(2) > 1 ? "std() not support 3D data!" : "std() not support complex value type!")
                << std::endl;
        return Matrix();
    }

    Matrix src = aMatrix.isComplex() ? Aurora::abs(aMatrix) : aMatrix.deepCopy();
    int calc_size = src.getDimSize(0) == 1 ? src.getDimSize(1) : src.getDimSize(0);
    int col = src.getDimSize(0) == 1?1:src.getDimSize(1) ;
    auto std = Aurora::malloc(col);
    auto meanM = Aurora::mean(aMatrix);
    for (int i = 0; i < col; ++i) {
        double *p = src.getData() + i * calc_size;
        
        double mean = meanM[i];
        vdSubI(calc_size, p, 1, &mean, 0, p, 1);
        vdSqr(calc_size, p, p);
        std[i] = cblas_dasum(calc_size, p, 1) / (calc_size - 1);
    }
    vdSqrt(col, std, std);
    return Matrix::New(std, 1, col, aMatrix.getDimSize(2));
}

Matrix Aurora::min(const Matrix &aMatrix, FunctionDirection direction, long& rowIdx, long& colIdx) {
    if (aMatrix.getDimSize(2)>1 || aMatrix.isComplex()) {
        std::cerr
                << (aMatrix.getDimSize(2) > 1 ? "min() not support 3D data!" : "min() not support complex value type!")
                << std::endl;
        return Matrix();
    }
    //针对向量行等于列
    if (direction == Column && aMatrix.getDimSize(0)==1){
        direction = All;
    }
    switch (direction)
    {
        case All: {
            Eigen::Map<Eigen::MatrixXd> retV(aMatrix.getData(), aMatrix.getDimSize(0), aMatrix.getDimSize(1));
            double *ret = malloc(1);
            ret[0] = retV.array().minCoeff(&rowIdx, &colIdx);
            return Matrix::New(ret, 1);
        }
        case Row:
        {
            Eigen::Map<Eigen::MatrixXd> srcMatrix(aMatrix.getData(),aMatrix.getDimSize(0),aMatrix.getDimSize(1));
            double * ret = malloc(aMatrix.getDimSize(0));

            if (aMatrix.getDimSize(0) == 1){
                ret[0] = srcMatrix.topRows(0).minCoeff(&rowIdx, &colIdx);
            }
            else{
                Eigen::Map<Eigen::MatrixXd> retMatrix(ret,aMatrix.getDimSize(0),1);
                retMatrix = srcMatrix.rowwise().minCoeff();
            }
            return Matrix::New(ret,aMatrix.getDimSize(0),1);
        }
        case Column:
        default:
        {
            Eigen::Map<Eigen::MatrixXd> srcMatrix(aMatrix.getData(),aMatrix.getDimSize(0),aMatrix.getDimSize(1));
            double * ret = malloc(aMatrix.getDimSize(1));

            if (aMatrix.getDimSize(1) == 1){
                ret[0] = srcMatrix.col(0).minCoeff(&rowIdx, &colIdx);
            }
            else {
                Eigen::Map<Eigen::MatrixXd> retMatrix(ret,1,aMatrix.getDimSize(1));
                retMatrix = srcMatrix.colwise().minCoeff();
            }
            return Matrix::New(ret,1,aMatrix.getDimSize(1));
        }
    }
}

Matrix Aurora::min(const Matrix &aMatrix, FunctionDirection direction) {
    long a,b;
    return  min(aMatrix,direction,a,b);
}

Matrix Aurora::min(const Matrix &aMatrix, const Matrix &aOther) {
    if (aMatrix.getDimSize(2)>1 || aMatrix.isComplex()) {
        std::cerr
                << (aMatrix.getDimSize(2) > 1 ? "min() not support 3D data!" : "min() not support complex value type!")
                << std::endl;
        return Matrix();
    }
    if (aOther.getDimSize(2)>1 || aOther.isComplex()) {
        std::cerr
                << (aOther.getDimSize(2) > 1 ? "min() not support 3D data!" : "min() not support complex value type!")
                << std::endl;
        return Matrix();
    }
    //same shape
    if (aMatrix.compareShape(aOther)){
        double* output = malloc(aMatrix.getDataSize());
        vdFminI(aMatrix.getDataSize(),aMatrix.getData(),1,aOther.getData(),1,output,1);
        return Matrix::New(output,aMatrix);
    }
    // one is scalar
    else if (aMatrix.getDataSize() == 1 || aOther.getDataSize() == 1){
            double scalar = (aMatrix.getDataSize() == 1)?aMatrix.getData()[0]:aOther.getData()[0];
            auto matrix = (aMatrix.getDataSize() == 1)?aOther:aMatrix;
            double* output = malloc(matrix.getDataSize());
            vdFminI(matrix.getDataSize(),matrix.getData(),1,&scalar,0,output,1);
            return Matrix::New(output,matrix);
    }
    else if (aMatrix.getDimSize(1) == 1 || aOther.getDimSize(0) == 1) {
        if (aMatrix.getDimSize(1) == 1){
            double* output = malloc(aOther.getDataSize());
            for (int i = 0; i < aOther.getDimSize(1); ++i) {
                vdFminI(aMatrix.getDataSize(), aMatrix.getData(), 1, aOther.getData() + aOther.getDimSize(0) * i, 1,
                        output + aOther.getDimSize(0) * i, 1);
            }
            return Matrix::New(output,aOther);
        }
        else{
            double* output = malloc(aMatrix.getDataSize());
            for (int i = 0; i < aMatrix.getDimSize(0); ++i) {
                vdFminI(aOther.getDataSize(), aOther.getData(), 1, aMatrix.getData() +  i, aMatrix.getDimSize(0),
                        output +  i, aOther.getDimSize(0));
            }
            return Matrix::New(output,aMatrix);
        }
    }
    else{
        std::cerr
                << "min(A,B) with matrix must be like A[MxN] - B[1xN] or A[Mx1] - B[MxN]"
                << std::endl;
        return Matrix();
    }
}

Matrix Aurora::max(const Matrix &aMatrix, FunctionDirection direction) {
    long a,b;
    return  max(aMatrix,direction,a,b);
}

Matrix Aurora::max(const Matrix &aMatrix, FunctionDirection direction, long& rowIdx, long& colIdx) {
    if (aMatrix.getDimSize(2)>1) {
        std::cerr
                << "max() not support 3D data!"
                << std::endl;
        return Matrix();
    }
    auto calcMatrix = aMatrix.isComplex()?abs(aMatrix):aMatrix;

    //针对向量行等于列
    if (direction == Column && calcMatrix.getDimSize(0)==1){
        direction = All;
    }
    switch (direction)
    {
        case All:
        {
            Eigen::Map<Eigen::MatrixXd> retV(calcMatrix.getData(), calcMatrix.getDimSize(0), calcMatrix.getDimSize(1));
            double *ret = malloc(1);
            ret[0] = retV.array().maxCoeff(&rowIdx, &colIdx);
            return Matrix::New(ret,1);
        }
        case Row:
        {
            Eigen::Map<Eigen::MatrixXd> srcMatrix(calcMatrix.getData(),calcMatrix.getDimSize(0),calcMatrix.getDimSize(1));
            double * ret = malloc(calcMatrix.getDimSize(0));
            if (calcMatrix.getDimSize(0) == 1){
                ret[0] = srcMatrix.topRows(0).maxCoeff(&rowIdx, &colIdx);
            }
            else{
                Eigen::Map<Eigen::MatrixXd> retMatrix(ret,calcMatrix.getDimSize(0),1);
                retMatrix = srcMatrix.rowwise().maxCoeff();
            }
            return Matrix::New(ret,calcMatrix.getDimSize(0),1);
        }
        case Column:
        default:
        {
            Eigen::Map<Eigen::MatrixXd> srcMatrix(calcMatrix.getData(),calcMatrix.getDimSize(0),calcMatrix.getDimSize(1));
            double * ret = malloc(calcMatrix.getDimSize(1));
            if (calcMatrix.getDimSize(1) == 1){
                ret[0] = srcMatrix.col(0).maxCoeff(&rowIdx, &colIdx);
            }
            else {
                Eigen::Map<Eigen::MatrixXd> retMatrix(ret,1,calcMatrix.getDimSize(1));
                retMatrix = srcMatrix.colwise().maxCoeff();
            }
            return Matrix::New(ret,1,calcMatrix.getDimSize(1));
        }
    }
}

Matrix Aurora::max(const Matrix &aMatrix, const Matrix &aOther) {
    if (aMatrix.getDimSize(2)>1 || aMatrix.isComplex()) {
        std::cerr
                << (aMatrix.getDimSize(2) > 1 ? "min() not support 3D data!" : "min() not support complex value type!")
                << std::endl;
        return Matrix();
    }
    if (aOther.getDimSize(2)>1 || aOther.isComplex()) {
        std::cerr
                << (aOther.getDimSize(2) > 1 ? "min() not support 3D data!" : "min() not support complex value type!")
                << std::endl;
        return Matrix();
    }
    //same shape
    if (aMatrix.compareShape(aOther)){
        double* output = malloc(aMatrix.getDataSize());
        vdFmaxI(aMatrix.getDataSize(),aMatrix.getData(),1,aOther.getData(),1,output,1);
        return Matrix::New(output,aMatrix);
    }
    // one is scalar
    else if (aMatrix.getDataSize() == 1 || aOther.getDataSize() == 1){
            double scalar = (aMatrix.getDataSize() == 1)?aMatrix.getData()[0]:aOther.getData()[0];
            auto matrix = (aMatrix.getDataSize() == 1)?aOther:aMatrix;
            double* output = malloc(matrix.getDataSize());
            vdFmaxI(matrix.getDataSize(),matrix.getData(),1,&scalar,0,output,1);
            return Matrix::New(output,matrix);
    }
    else if (aMatrix.getDimSize(1) == 1 || aOther.getDimSize(0) == 1) {
        if (aMatrix.getDimSize(1) == 1){
            double* output = malloc(aOther.getDataSize());
            for (int i = 0; i < aOther.getDimSize(1); ++i) {
                vdFmaxI(aMatrix.getDataSize(), aMatrix.getData(), 1, aOther.getData() + aOther.getDimSize(0) * i, 1,
                        output + aOther.getDimSize(0) * i, 1);
            }
            return Matrix::New(output,aOther);
        }
        else{
            double* output = malloc(aMatrix.getDataSize());
            for (int i = 0; i < aMatrix.getDimSize(0); ++i) {
                vdFmaxI(aOther.getDataSize(), aOther.getData(), 1, aMatrix.getData() +  i, aMatrix.getDimSize(0),
                        output +  i, aOther.getDimSize(0));
            }
            return Matrix::New(output,aMatrix);
        }
    }
    else{
        std::cerr
                << "min(A,B) with matrix must be like A[MxN] - B[1xN] or A[Mx1] - B[MxN]"
                << std::endl;
        return Matrix();
    }
}

Matrix Aurora::max(const Matrix &aMatrix, const double aValue){
    double *output = malloc(1);
    output[0] = aValue;
    return max(aMatrix,Matrix::New(output, 1,1,1));
}

Matrix Aurora::sum(const Matrix &aMatrix, FunctionDirection direction) {
    if (aMatrix.getDimSize(2)>1 ) {
        std::cerr<<  "sum() not support 3D data!"
                << std::endl;
        return Matrix();
    }
    //针对向量行等于列
    if (direction == Column && aMatrix.getDimSize(0)==1){
        direction = Row;
    }
    if (aMatrix.isComplex()){
        switch (direction)
        {
            case All:
            {
                Eigen::Map<Eigen::VectorXcd> srcV((std::complex<double>*)aMatrix.getData(),aMatrix.getDataSize());
                std::complex<double>* ret = (std::complex<double>*)malloc(1,true);
                ret[0] = srcV.array().sum();
                return Matrix::New((double*)ret,1,1,1,Complex);
            }
            case Row:
            {
                Eigen::Map<Eigen::MatrixXcd> srcM((std::complex<double>*)aMatrix.getData(),aMatrix.getDimSize(0),aMatrix.getDimSize(1));
                std::complex<double> * ret = (std::complex<double>*)malloc(aMatrix.getDimSize(0),true);
                Eigen::Map<Eigen::VectorXcd> retV(ret,aMatrix.getDimSize(0));
                retV = srcM.rowwise().sum();
                return Matrix::New((double*)ret,aMatrix.getDimSize(0),1,1,Complex);
            }
            case Column:
            default:
            {
                Eigen::Map<Eigen::MatrixXcd> srcM((std::complex<double>*)aMatrix.getData(),aMatrix.getDimSize(0),aMatrix.getDimSize(1));
                std::complex<double>* ret = (std::complex<double>*)malloc(aMatrix.getDimSize(1),true);
                Eigen::Map<Eigen::VectorXcd> retV(ret,aMatrix.getDimSize(1));
                retV = srcM.colwise().sum();
                return Matrix::New((double*)ret,1,aMatrix.getDimSize(1),1,Complex);
            }
        }
    }
    else{
        switch (direction)
        {
            case All:
            {
                Eigen::Map<Eigen::VectorXd> srcV(aMatrix.getData(),aMatrix.getDataSize());
                double * ret = malloc(1);
                ret[0] = srcV.array().sum();
                return Matrix::New(ret,1);
            }
            case Row:
            {
                Eigen::Map<Eigen::MatrixXd> srcM(aMatrix.getData(),aMatrix.getDimSize(0),aMatrix.getDimSize(1));
                double * ret = malloc(aMatrix.getDimSize(0));
                Eigen::Map<Eigen::VectorXd> retV(ret,aMatrix.getDimSize(0));
                retV = srcM.rowwise().sum();
                return Matrix::New(ret,aMatrix.getDimSize(0),1);
            }
            case Column:
            default:
            {
                Eigen::Map<Eigen::MatrixXd> srcM(aMatrix.getData(),aMatrix.getDimSize(0),aMatrix.getDimSize(1));
                double * ret = malloc(aMatrix.getDimSize(1));
                Eigen::Map<Eigen::VectorXd> retV(ret,aMatrix.getDimSize(1));
                retV = srcM.colwise().sum();
                return Matrix::New(ret,1,aMatrix.getDimSize(1));
            }
        }
    }
}

Matrix Aurora::mean(const Matrix &aMatrix, FunctionDirection direction, bool aIncludeNan) {
    if (aMatrix.getDimSize(2)>1 || aMatrix.isComplex()) {
        std::cerr
                << (aMatrix.getDimSize(2) > 1 ? "sum() not support 3D data!" : "sum() not support complex value type!")
                << std::endl;
        return Matrix();
    }
    //针对向量行等于列
    if (direction == Column && aMatrix.getDimSize(0)==1){
        direction = All;
    }
    if (aIncludeNan){
        switch (direction)
        {
            case All:
            {
                Eigen::Map<Eigen::VectorXd> srcV(aMatrix.getData(),aMatrix.getDataSize());
                double * ret = malloc(64);
                ret[0] = srcV.array().mean();
                return Matrix::New(ret,1);
            }
            case Row:
            {
                Eigen::Map<Eigen::MatrixXd> srcM(aMatrix.getData(),aMatrix.getDimSize(0),aMatrix.getDimSize(1));
                double * ret = malloc(aMatrix.getDimSize(0));
                Eigen::Map<Eigen::VectorXd> retV(ret,aMatrix.getDimSize(0));
                retV = srcM.rowwise().mean();
                return Matrix::New(ret,aMatrix.getDimSize(0),1);
            }
            case Column:
            default:
            {
                Eigen::Map<Eigen::MatrixXd> srcM(aMatrix.getData(),aMatrix.getDimSize(0),aMatrix.getDimSize(1));
                double * ret = malloc(aMatrix.getDimSize(1));
                Eigen::Map<Eigen::VectorXd> retV(ret,aMatrix.getDimSize(1));
                retV = srcM.colwise().mean();
                return Matrix::New(ret,1,aMatrix.getDimSize(1));
            }
        }
    }
    else{
        switch (direction)
        {
            case All:
            {
                Eigen::Map<Eigen::VectorXd> srcV(aMatrix.getData(),aMatrix.getDataSize());
                double * retVd = malloc(aMatrix.getDataSize());
                Eigen::Map<Eigen::VectorXd> retV(retVd,aMatrix.getDataSize());
                Eigen::VectorXd ones = Eigen::VectorXd(aMatrix.getDataSize());
                ones.setConstant(1.0);
                retV = srcV.array().isNaN().select(0.0,ones);
                int count = retV.sum();
                if (count == 0){
                    free(retVd);
                    double *ret = malloc(1);
                    ret[0]=0;
                    return Matrix::New(ret,1);
                }
                else {
                    double *ret = malloc(1);
                    retV = srcV.array().isNaN().select(0.0,srcV);
                    ret[0] = retV.sum() / count;
                    free(retVd);
                    return Matrix::New(ret, 1);
                }
            }
            case Row:
            {
                Eigen::Map<Eigen::MatrixXd> srcM(aMatrix.getData(),aMatrix.getDimSize(0),aMatrix.getDimSize(1));
                double * retMd = malloc(aMatrix.getDataSize());
                Eigen::Map<Eigen::MatrixXd> retM(retMd,aMatrix.getDimSize(0),aMatrix.getDimSize(1));
                Eigen::MatrixXd zeros = Eigen::MatrixXd(aMatrix.getDimSize(0), aMatrix.getDimSize(1));
                zeros.setConstant(0.0);
                retM = srcM.array().isNaN().select(zeros,1.0);
                Eigen::VectorXd countM = retM.rowwise().sum();
                countM = (countM.array()==0.0).select(1.0,countM);
                retM = srcM.array().isNaN().select(0.0,srcM);
                double * ret = malloc(aMatrix.getDimSize(0));
                Eigen::Map<Eigen::VectorXd> retV(ret,aMatrix.getDimSize(0));
                retV = retM.rowwise().sum();
                retV =retV.array()/countM.array();
                free(retMd);
                return Matrix::New(ret,aMatrix.getDimSize(0),1);
            }
            case Column:
            default:
            {
                Eigen::Map<Eigen::MatrixXd> srcM(aMatrix.getData(),aMatrix.getDimSize(0),aMatrix.getDimSize(1));
                double * retMd = malloc(aMatrix.getDataSize());
                Eigen::Map<Eigen::MatrixXd> retM(retMd,aMatrix.getDimSize(0),aMatrix.getDimSize(1));
                Eigen::MatrixXd zeros = Eigen::MatrixXd(aMatrix.getDimSize(0), aMatrix.getDimSize(1));
                zeros.setConstant(0.0);
                retM = srcM.array().isNaN().select(zeros,1.0);
                Eigen::VectorXd countM = retM.colwise().sum();
                countM = (countM.array()==0).select(1.0,countM);
                retM = srcM.array().isNaN().select(0.0,srcM);
                double * ret = malloc(aMatrix.getDimSize(1));
                Eigen::Map<Eigen::VectorXd> retV(ret,aMatrix.getDimSize(1));
                retV = retM.colwise().sum();
                retV = retV.array()/countM.array();
                free(retMd);
                return Matrix::New(ret,1,aMatrix.getDimSize(1));
            }
        }
    }
}

Matrix Aurora::sort(const Matrix &aMatrix, FunctionDirection direction) {
    if (aMatrix.getDimSize(2)>1 || aMatrix.isComplex()) {
        std::cerr
                << (aMatrix.getDimSize(2) > 1 ? "sort() not support 3D data!" : "sort() not support complex value type!")
                << std::endl;
        return Matrix();
    }
    return sort(std::forward<Matrix &&>(aMatrix.deepCopy()), direction);
}

Matrix Aurora::sort(Matrix &&aMatrix, FunctionDirection direction) {
    if (aMatrix.getDimSize(2)>1 || aMatrix.isComplex()) {
        std::cerr
                << (aMatrix.getDimSize(2) > 1 ? "sort() not support 3D data!" : "sort() not support complex value type!")
                << std::endl;
        return Matrix();
    }
    //针对向量行等于列
    if (aMatrix.getDimSize(0)==1){
        direction = Row;
    }
    if (direction == Column)
    {
        if (aMatrix.getDimSize(0)>=100000){
            #pragma omp parallel for
            for (int i = 0; i < aMatrix.getDimSize(1); ++i) {
                Eigen::Map<Eigen::VectorXd> srcV(aMatrix.getData()+i*aMatrix.getDimSize(0),aMatrix.getDimSize(0));
                std::sort(srcV.array().begin(),srcV.array().end());
            }
        }
        else
        {
            for (int i = 0; i < aMatrix.getDimSize(1); ++i) {
                Eigen::Map<Eigen::VectorXd> srcV(aMatrix.getData()+i*aMatrix.getDimSize(0),aMatrix.getDimSize(0));
                std::sort(srcV.array().begin(),srcV.array().end());
            }
        }
    }
    else if(direction == Row){
        Eigen::Map<Eigen::MatrixXd> srcM(aMatrix.getData(),aMatrix.getDimSize(0),aMatrix.getDimSize(1));
        if (aMatrix.getDimSize(1)>=100000){
            #pragma omp parallel for
            for (int i = 0; i < aMatrix.getDimSize(0); ++i) {
                std::sort(srcM.row(i).array().begin(),srcM.row(i).array().end());
            }
        }
        else
        {
            for (int i = 0; i < aMatrix.getDimSize(0); ++i) {
                std::sort(srcM.row(i).array().begin(),srcM.row(i).array().end());
            }
        }
    }
    else{
        std::cerr<<"sort not support all mode!"<<std::endl;
    }
    return aMatrix;
}

Matrix Aurora::sortrows(const Matrix &aMatrix, Matrix* indexMatrix) {
    if (aMatrix.getDimSize(2)>1 || aMatrix.isComplex()) {
        std::cerr
                << (aMatrix.getDimSize(2) > 1 ? "sortrows() not support 3D data!" : "sortrows() not support complex value type!")
                << std::endl;
        return Matrix();
    }
    auto result = aMatrix.deepCopy();
    int rows = aMatrix.getDimSize(0);
    std::vector<std::pair<double,int>> col;
    std::vector<std::pair<double,int>>::iterator last;
    for (size_t j = 0; j < rows; j++)
    {
        col.push_back(std::make_pair(aMatrix[j], j));
    }
    std::sort(col.begin(), col.end(),[](auto a,auto b){
        return a.first < b.first;
    });
    last = col.begin();
    //按列里边
    for (size_t i = 1; i < aMatrix.getDimSize(1); i++)
    {
        int beginIdx = 0;
        bool sameFlag = false;
        //遍历已排序数据查找相同值
        while(beginIdx < col.size()){
            for (int iterIdx = beginIdx+1; iterIdx <= col.size(); iterIdx++)
            {
                //查找下一个不同值
                if(col[iterIdx].first == col[iterIdx-1].first && iterIdx!=col.size()){
                    //存在相同值
                    sameFlag = true;
                    continue;
                }
                //判断是否需要对相同值进行排序，iterIdx-beginIdx==1时代表正常的不同值
                if (iterIdx-beginIdx != 1){
                    //按照新的一列对相同值排序
                    std::sort(col.begin()+beginIdx, col.begin()+iterIdx,[&aMatrix,i,rows](auto a,auto b){
                        return aMatrix[a.second+i*rows] < aMatrix[b.second+i*rows];
                    });
                }
                beginIdx = iterIdx;
            }
            //未发现不同值 break 循环
            if(!sameFlag) break;
        }
        //未发现不同值 break 循环
        if(!sameFlag) break;
        //按照新一列刷新数组值
        std::for_each(col.begin(), col.end() , [&aMatrix,i,rows](std::pair<double,int>& a){return a.first=aMatrix[a.second+i*rows];});
    }
    int i=0;
    (*indexMatrix) = zeros(aMatrix.getDimSize(0),1);
    std::for_each(col.begin(),col.end(), [&aMatrix,&result,&i,indexMatrix](std::pair<double,int>& a){ 
        result(i,$) = aMatrix(a.second,$);
        (*indexMatrix)[i] = a.second;
        i++;
    });
    return result;
}

Matrix Aurora::median(const Matrix &aMatrix) {
    if (aMatrix.getDimSize(2) > 1 || aMatrix.isComplex()) {
        std::cerr
                << (aMatrix.getDimSize(2) > 1 ? "median() not support 3D data!"
                                              : "median() not support complex value type!")
                << std::endl;
        return Matrix();
    }
    bool horVector = aMatrix.getDimSize(0)==1;
    Matrix sorted = horVector?sort(aMatrix,Row):sort(aMatrix);
    int rows = horVector?sorted.getDimSize(1):sorted.getDimSize(0);
    int cols = horVector?sorted.getDimSize(0):sorted.getDimSize(1);
    Eigen::Map<Eigen::MatrixXd> srcM(sorted.getData(),rows,cols);
    bool flag = rows % 2 == 1;
    double* ret = malloc(cols);
    Eigen::Map<Eigen::VectorXd> retV(ret,cols);
    if (flag) {
        retV = srcM.row(rows/2);
        return Matrix::New(ret,1,cols);
    } else {
        retV = (srcM.row(rows/2-1).array()+srcM.row(rows/2).array())/2;
        return Matrix::New(ret,1,cols);
    }
}

Matrix Aurora::fft(const Matrix &aMatrix, long aFFTSize) {
    double *output = nullptr;
    mkl_free_buffers();
    MKL_LONG rowSize = (aFFTSize>0)?aFFTSize:aMatrix.getDimSize(0);
    //实际需要copy赋值的非0值
    MKL_LONG needCopySize = (rowSize<aMatrix.getDimSize(0))?rowSize:aMatrix.getDimSize(0);
    MKL_LONG bufferSize = rowSize*aMatrix.getDimSize(1);
    output = malloc(bufferSize, true);
    double zero = 0.0;
    //先全部置为0
    cblas_dcopy(bufferSize*2, &zero, 0, output, 1);
    if (!aMatrix.isComplex()) {
        //按列copy原值
        for (int i = 0 ; i < aMatrix.getDimSize(1); ++i) {
            cblas_dcopy(needCopySize, aMatrix.getData()+i*aMatrix.getDimSize(0), 1, output+i*rowSize*2, 2);
        }
    } else {
        //按列copy原值
        for (int i = 0 ; i < aMatrix.getDimSize(1); ++i) {
            cblas_zcopy(needCopySize, aMatrix.getData()+i*aMatrix.getDimSize(0)*2, 1, output+i*rowSize*2, 1);
        }
    }

    DFTI_DESCRIPTOR_HANDLE my_desc_handle = NULL;
    MKL_LONG status;

    //创建 Descriptor, 精度 double , 输入类型实数, 维度1
    status = DftiCreateDescriptor(&my_desc_handle, DFTI_DOUBLE, DFTI_COMPLEX, 1, rowSize);
    if (status != DFTI_NO_ERROR) goto error;

    //通过 setValue 配置Descriptor
    //使用单独的输出数据缓存
    status = DftiSetValue(my_desc_handle, DFTI_PLACEMENT, DFTI_INPLACE);
    if (status != DFTI_NO_ERROR) goto error;

    //每个傅里叶变换的输入距离
    status = DftiSetValue(my_desc_handle,DFTI_INPUT_DISTANCE,rowSize);
    if (status != DFTI_NO_ERROR) goto error;

    //每个傅里叶变换的输出距离
    status = DftiSetValue(my_desc_handle,DFTI_OUTPUT_DISTANCE,rowSize);
    if (status != DFTI_NO_ERROR) goto error;

    //傅里叶变换的数量
    status = DftiSetValue(my_desc_handle,DFTI_NUMBER_OF_TRANSFORMS,aMatrix.getDimSize(1));
    if (status != DFTI_NO_ERROR) goto error;

    //提交 修改配置后的Descriptor(实际上会进行FFT的计算初始化)
    status = DftiCommitDescriptor(my_desc_handle);
    if (status != DFTI_NO_ERROR) goto error;

    //执行计算
    status = DftiComputeForward(my_desc_handle, output, output);
    if (status != DFTI_NO_ERROR) goto error;

    //释放资源
    status = DftiFreeDescriptor(&my_desc_handle);
    if (status != DFTI_NO_ERROR) goto error;
    mkl_free_buffers();
    return Matrix::New(output, rowSize, aMatrix.getDimSize(1), aMatrix.getDimSize(2), Complex);
    error:
        std::cerr<<"FFT fail, error message:"<<DftiErrorMessage(status)<<std::endl;
        return Matrix();
}

Matrix Aurora::ifft(const Matrix &aMatrix, long aFFTSize ) {
    if (!aMatrix.isComplex()){
        std::cerr<<"ifft input must be complex value"<<std::endl;
        return Matrix();
    }
    DFTI_DESCRIPTOR_HANDLE my_desc_handle = NULL;
    // mkl_free_buffers();
    // auto output = malloc(aMatrix.getDataSize(),true);
    MKL_LONG status;
    double *output = nullptr;
    mkl_free_buffers();
    MKL_LONG rowSize = (aFFTSize>0)?aFFTSize:aMatrix.getDimSize(0);
    //实际需要copy赋值的非0值
    MKL_LONG needCopySize = (rowSize<aMatrix.getDimSize(0))?rowSize:aMatrix.getDimSize(0);
    MKL_LONG bufferSize = rowSize*aMatrix.getDimSize(1);
    output = malloc(bufferSize, true);
    double zero = 0.0;
    //先全部置为0
    cblas_dcopy(bufferSize*2, &zero, 0, output, 1);
    if (!aMatrix.isComplex()) {
        //按列copy原值
        for (int i = 0 ; i < aMatrix.getDimSize(1); ++i) {
            cblas_dcopy(needCopySize, aMatrix.getData()+i*aMatrix.getDimSize(0), 1, output+i*rowSize*2, 2);
        }
    } else {
        //按列copy原值
        for (int i = 0 ; i < aMatrix.getDimSize(1); ++i) {
            cblas_zcopy(needCopySize, aMatrix.getData()+i*aMatrix.getDimSize(0)*2, 1, output+i*rowSize*2, 1);
        }
    }
    //创建 Descriptor, 精度 double , 输入类型实数, 维度1
    int size = aMatrix.getDimSize(0);
    status = DftiCreateDescriptor(&my_desc_handle, DFTI_DOUBLE, DFTI_COMPLEX, 1, rowSize);
    if (status != DFTI_NO_ERROR) goto error;
    //通过 setValue 配置Descriptor
    //使用单独的输出数据缓存
    status = DftiSetValue(my_desc_handle, DFTI_PLACEMENT, DFTI_NOT_INPLACE);
    if (status != DFTI_NO_ERROR) goto error;

    //设置DFTI_BACKWARD_SCALE ！！！很关键，不然值不对
    status = DftiSetValue(my_desc_handle, DFTI_BACKWARD_SCALE, 1.0f / rowSize);
    if (status != DFTI_NO_ERROR) goto error;

    status = DftiSetValue(my_desc_handle,DFTI_INPUT_DISTANCE,rowSize);
    if (status != DFTI_NO_ERROR) goto error;

    //每个傅里叶变换的输出距离
    status = DftiSetValue(my_desc_handle,DFTI_OUTPUT_DISTANCE,rowSize);
    if (status != DFTI_NO_ERROR) goto error;

    //傅里叶变换的数量
    status = DftiSetValue(my_desc_handle,DFTI_NUMBER_OF_TRANSFORMS,aMatrix.getDimSize(1));
    if (status != DFTI_NO_ERROR) goto error;

    status = DftiCommitDescriptor(my_desc_handle);
    if (status != DFTI_NO_ERROR) goto error;

    //提交 修改配置后的Descriptor(实际上会进行FFT的计算初始化)
    status = DftiCommitDescriptor(my_desc_handle);
    if (status != DFTI_NO_ERROR) goto error;
    
    //执行计算
    status = DftiComputeBackward(my_desc_handle, aMatrix.getData(), output);
    if (status != DFTI_NO_ERROR) goto error;
    //释放资源
    status = DftiFreeDescriptor(&my_desc_handle);
    mkl_free_buffers();
    if (status != DFTI_NO_ERROR) goto error;
    {
        return Matrix::New(output, rowSize, aMatrix.getDimSize(1), aMatrix.getDimSize(2), Complex);
    }
    error:
    std::cerr<<"FFT fail, error message:"<<DftiErrorMessage(status)<<std::endl;
    return Matrix();
}

Matrix Aurora::ifft_symmetric(const Matrix &aMatrix,long length)
{
    if(!aMatrix.isVector()){
        std::cerr<<"ifft_symmetric only support vector!"<<std::endl;
        return Matrix();
    }
    int matrixLength = aMatrix.getDataSize();
    int resultHalfLength = (int)std::ceil(((double)length*0.5));
    int copyLength = resultHalfLength<matrixLength?resultHalfLength:matrixLength;
    double* calcData = malloc(length,true);
    double zero = 0.0;
    //所有数据统一置0
    cblas_dcopy(length*2,&zero,0,calcData,1);
    //copy前半段数据
    cblas_zcopy(copyLength,aMatrix.getData(),1,calcData,1);
    //copy后半段数据,跳过index 0的值,并设置虚部共轭
    vdAddI(copyLength-1,&zero,0,(aMatrix.getData()+2),2,(calcData+(length-1)*2),-2);
    vdSubI(copyLength-1,&zero,0,(aMatrix.getData()+2+1),2,(calcData+(length-1)*2+1),-2);

    return real(ifft(Matrix::New(calcData,length,1,1,Complex)));
}

void Aurora::fftshift(Matrix &aMatrix){
    int backwardLength = aMatrix.getDimSize(0)/2;
    if (aMatrix.getDimSize(0)%2!=0)++backwardLength;
    int forwardLength = aMatrix.getDimSize(0) - backwardLength;
    double* buffer = malloc(forwardLength,true);
    int valueStep = aMatrix.isComplex()?2:1;
    for (int i = 0; i<aMatrix.getDimSize(1); ++i) {
        double* dataPtr = aMatrix.getData()+aMatrix.getDimSize(0)*i*valueStep;
        cblas_dcopy(forwardLength*valueStep, dataPtr+backwardLength*valueStep, 1, buffer, 1);
        cblas_dcopy(backwardLength*valueStep, dataPtr, 1, dataPtr+forwardLength*valueStep, 1);
        cblas_dcopy(forwardLength*valueStep, buffer, 1, dataPtr, 1);
    }
    Aurora::free(buffer);
}

void Aurora::ifftshift(Matrix &aMatrix){
    int forwardLength= aMatrix.getDimSize(0)/2;
    if (aMatrix.getDimSize(0)%2!=0)++forwardLength;
    int backwardLength = aMatrix.getDimSize(0) - forwardLength;
    double* buffer = malloc(forwardLength,true);
    int valueStep = aMatrix.isComplex()?2:1;
    for (int i = 0; i<aMatrix.getDimSize(1); ++i) {
        double* dataPtr = aMatrix.getData()+aMatrix.getDimSize(0)*i*valueStep;
        cblas_dcopy(forwardLength*valueStep, dataPtr+backwardLength*valueStep, 1, buffer, 1);
        cblas_dcopy(backwardLength*valueStep, dataPtr, 1, dataPtr+forwardLength*valueStep, 1);
        cblas_dcopy(forwardLength*valueStep, buffer, 1, dataPtr, 1);
    }
    Aurora::free(buffer);
}

Matrix Aurora::hilbert(const Matrix &aMatrix) {
    auto x = fft(aMatrix);
    auto h = malloc(aMatrix.getDimSize(0));
    auto two = 2.0;
    auto zero = 0.0;
    cblas_dcopy(aMatrix.getDimSize(0), &zero, 0, h, 1);
    cblas_dcopy(aMatrix.getDimSize(0) / 2, &two, 0, h, 1);
    h[aMatrix.getDimSize(0) / 2] = ((aMatrix.getDimSize(0) << 31) >> 31) ? 2.0 : 1.0;
    h[0] = 1.0;
    for (int i = 0; i < aMatrix.getDimSize(1); ++i) {
        auto p = (double *)(x.getData() + aMatrix.getDimSize(0)* i*2);
        vdMulI(aMatrix.getDimSize(0), p, 2, h, 1, p, 2);
        vdMulI(aMatrix.getDimSize(0), p + 1, 2, h, 1, p + 1, 2);
    }
    auto result = ifft( x);
    free(h);
    return result;
}

Matrix Aurora::prod(const Matrix &aMatrix) {
    if (aMatrix.getDimSize(2) > 1 ) {
        std::cerr<< "prod() not support 3D data!"<< std::endl;
        return Matrix();
    }
    int row = aMatrix.getDimSize(0)==1?aMatrix.getDimSize(1):aMatrix.getDimSize(0);
    int col = aMatrix.getDimSize(0)==1?1:aMatrix.getDimSize(1);
    if (aMatrix.isComplex()){
        Eigen::Map<Eigen::MatrixXcd> srcM((std::complex<double>*)aMatrix.getData(),row,col);
        auto ret = malloc(col,true);
        Eigen::Map<Eigen::VectorXcd> retV((std::complex<double>*)ret,col);
        retV = srcM.colwise().prod();
        return Matrix::New(ret,1,col,1,Complex);
    }
    else{
        Eigen::Map<Eigen::MatrixXd> srcM(aMatrix.getData(),row,col);
        auto ret = malloc(col);
        Eigen::Map<Eigen::VectorXd> retV(ret,col);
        retV = srcM.colwise().prod();
        return Matrix::New(ret,1,col);
    }
}

Matrix Aurora::dot(const Matrix &aMatrix,const Matrix& aOther,FunctionDirection direction ) {
    if ( direction == All){
        std::cerr<< "dot() not support 3D data!"<< std::endl;
        return Matrix();
    }
    if (!aMatrix.compareShape(aOther)){
        std::cerr<< "dot() matrix must be same shape!"<< std::endl;
        return Matrix();
    }
    //针对向量行等于列
    if (direction == Column && aMatrix.getDimSize(0)==1){
        direction = Row;
    }
    if (aMatrix.isComplex()){
        return sum(conj(aMatrix)*aOther,direction);
    }
    else{
        if (direction == Column)
        {
            auto ret = malloc(aMatrix.getDimSize(1));
            for (int i = 0; i < aMatrix.getDimSize(1); ++i) {
                ret[i]=cblas_ddot(aMatrix.getDimSize(0),aMatrix.getData()+i*aMatrix.getDimSize(0),1,
                                aOther.getData()+i*aMatrix.getDimSize(0),1);
            }
            return Matrix::New(ret,1,aMatrix.getDimSize(1),1);
        }
        else{
            auto ret = malloc(aMatrix.getDimSize(0));
            for (int i = 0; i < aMatrix.getDimSize(0); ++i) {
                ret[i] = cblas_ddot(aMatrix.getDimSize(1),aMatrix.getData()+i,aMatrix.getDimSize(0),
                                aOther.getData()+i,aMatrix.getDimSize(0));
            }
            return Matrix::New(ret,aMatrix.getDimSize(1),1,1);
        }
    }
}


Matrix Aurora::sub2ind(const Matrix &aVMatrixSize, std::initializer_list<Matrix> aSliceIdxsList)
{
    if (aSliceIdxsList.size() != aVMatrixSize.getDataSize()){
        std::cerr<<"sub2ind size not match"<<std::endl;
        return Matrix();
    }
    if (aSliceIdxsList.size() == 0){
        std::cerr<<"sub2ind no index need calc!"<<std::endl;
        return Matrix();
    }
    size_t returnVectorSize = aSliceIdxsList.begin()->getDataSize();
    double *strides =new double[aVMatrixSize.getDataSize()+1];
    strides[0] = 1;
    for (int i = 1; i<=aVMatrixSize.getDataSize(); ++i) {
        strides[i] = strides[i-1]*aVMatrixSize.getData()[i-1];
    } 
    double* output = Aurora::malloc(returnVectorSize);
    for (size_t i = 0; i<returnVectorSize; ++i) {
        size_t j = 0;
        std::for_each(aSliceIdxsList.begin(), aSliceIdxsList.end(), [strides,output,i,&aVMatrixSize,&j](const Matrix& matrix){
            if(j == 0 ){
                output[i]= (matrix.getData()[i])*strides[j];
            }
            else{
                output[i]+= (matrix.getData()[i]-1)*strides[j];
            }
            ++j;
        });
    }
    return Matrix::New(output,returnVectorSize,1,1);
}