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

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

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) {
    auto std = Aurora::malloc(aMatrix.getDimSize(1) * aMatrix.getDimSize(2));
    Matrix src = aMatrix.isComplex() ? Aurora::abs(aMatrix) : aMatrix;
    for (int i = 0; i < src.getDimSize(1); ++i) {
        double *p = src.getData() + i * src.getDimSize(0);
        double mean = cblas_dasum(src.getDimSize(0), p, 1) / src.getDimSize(0);
        vdSubI(src.getDimSize(0), p, 1, &mean, 0, p, 1);
        vdSqr(src.getDimSize(0), p, p);
        std[i] = cblas_dasum(src.getDimSize(0), p, 1) / (src.getDimSize(0) - 1);
    }
    vdSqrt(src.getDimSize(1), std, std);

    return Matrix::New(std,1,aMatrix.getDimSize(1), aMatrix.getDimSize(2));
}

Matrix Aurora::min(const Matrix &aMatrix, FunctionDirection direction) {
    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();
    }
    switch (direction)
    {
        case All:
        {
            Eigen::Map<Eigen::VectorXd> retV(aMatrix.getData(),aMatrix.getDataSize());
            double * ret = malloc(1);
            ret[0] = retV.array().minCoeff();
            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));
            Eigen::Map<Eigen::MatrixXd> retMatrix(ret,aMatrix.getDimSize(0),1);
            retMatrix = srcMatrix.rowwise().minCoeff();
            return Matrix::New(ret,aMatrix.getDimSize(0),1);
        }
        case Column:
        {
            Eigen::Map<Eigen::MatrixXd> srcMatrix(aMatrix.getData(),aMatrix.getDimSize(0),aMatrix.getDimSize(1));
            double * ret = malloc(aMatrix.getDimSize(0));
            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, 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) {
    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();
    }
    switch (direction)
    {
        case All:
        {
            Eigen::Map<Eigen::VectorXd> retV(aMatrix.getData(),aMatrix.getDataSize());
            double * ret = malloc(1);
            ret[0] = retV.array().maxCoeff();
            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));
            Eigen::Map<Eigen::MatrixXd> retMatrix(ret,aMatrix.getDimSize(0),1);
            retMatrix = srcMatrix.rowwise().maxCoeff();
            return Matrix::New(ret,aMatrix.getDimSize(0),1);
        }
        case Column:
        {
            Eigen::Map<Eigen::MatrixXd> srcMatrix(aMatrix.getData(),aMatrix.getDimSize(0),aMatrix.getDimSize(1));
            double * ret = malloc(aMatrix.getDimSize(0));
            Eigen::Map<Eigen::MatrixXd> retMatrix(ret,1,aMatrix.getDimSize(1));
            retMatrix = srcMatrix.colwise().maxCoeff();
            return Matrix::New(ret,1,aMatrix.getDimSize(1));
        }
    }
}