#ifndef DESCSS_Matrix_h
#define DESCSS_Matrix_h

#include <iostream>
#define swap2(x, y) ((x)=(x)^(y),(y)=(x)^(y),(x)=(x)^(y))

class matrix {
public:
    matrix(int n, int m);
    matrix(const matrix&);
    ~matrix();

    void Input(double* p);
    void Output();
    int rows() const { return n; };
    int cols() const { return m; };

    double& operator()(int i, int j) const;
    matrix& operator=(const matrix&);
    matrix operator+(const matrix&) const;
    matrix operator*(const matrix&) const;
    matrix operator+(const double&) const;
    matrix operator*(const double&) const;
    matrix operator/(const double&) const;

    matrix I();
    matrix T();

protected:
    int n, m;
    double** element;
};

matrix::matrix(int n, int m) {
    if (m < 0 || n < 0) std::cout << "Rows and Cols must be >= 0 " << std::endl;
    if ((m == 0 || n == 0) && (m != 0 || n != 0))
        std::cout << "Either both or neither rows and columns should be zero " << std::endl;
    this->m = m, this->n = n;
    element = new double*[n];
    for (int i = 0; i < n; i++) element[i] = new double[m];
    for (int i = 0; i < n; i++)
        for (int j = 0; j < m; j++) element[i][j] = 0;
}

matrix::matrix(const matrix& A) {
    n = A.n, m = A.m;
    element = new double*[n];
    for (int i = 0; i < n; i++) element[i] = new double[m];
    for (int i = 0; i < n; i++)
        for (int j = 0; j < m; j++) element[i][j] = A.element[i][j];
}

matrix::~matrix() {}

matrix& matrix::operator=(const matrix& A) {
    if (this != &A) {
        delete[] element;
        n = A.n, m = A.m;
        element = new double*[n];
        for (int i = 0; i < n; i++) element[i] = new double[m];
        for (int i = 0; i < n; i++)
            for (int j = 0; j < m; j++) element[i][j] = A.element[i][j];
    }
    return *this;
}

void matrix::Input(double* p) {
    for (int i = 0; i < n; i++)
        for (int j = 0; j < m; j++) this->element[i][j] = *(p++);
}

void matrix::Output() {
    for (int i = 0; i < n; i++) {
        for (int j = 0; j < m; j++) std::cout << element[i][j] << " ";
        std::cout << std::endl;
    }
}

double& matrix::operator()(int i, int j) const {
    if (i < 0 || i > n - 1 || j < 0 || j > m - 1) std::cout << "Matrix Index Out Of Bounds " << std::endl;
    return element[i][j];
}

matrix matrix::operator+(const matrix& A) const {
    if (n != A.n || m != A.m) std::cout << "Matrix Size is Out of batch " << std::endl;
    matrix w(n, m);
    for (int i = 0; i < n; i++)
        for (int j = 0; j < m; j++) w.element[i][j] = element[i][j] + A.element[i][j];
    return w;
}

matrix matrix::operator+(const double& C) const {
    matrix w(n, m);
    for (int i = 0; i < n; i++)
        for (int j = 0; j < m; j++) w.element[i][j] = element[i][j] + C;
    return w;
}

matrix matrix::operator*(const matrix& A) const {
    if (m != A.n) std::cout << "Matrix Style is Out of batch " << std::endl;
    matrix w(n, A.m);
    for (int i = 0; i < n; i++)
        for (int j = 0; j < A.m; j++) {
            w.element[i][j] = 0;
            for (int k = 0; k < m; k++) w.element[i][j] += element[i][k] * A.element[k][j];
        }
    return w;
}

matrix matrix::operator*(const double& C) const {
    matrix w(n, m);
    for (int i = 0; i < n; i++)
        for (int j = 0; j < m; j++) w.element[i][j] = element[i][j] * C;
    return w;
}

matrix matrix::operator/(const double& C) const {
    matrix w(n, m);
    for (int i = 0; i < n; i++)
        for (int j = 0; j < m; j++) w.element[i][j] = element[i][j] / C;
    return w;
}

BOOL LUPDescomposition(matrix mA, matrix& mL, matrix& mU, int* P)
{
    int n = mA.rows();
    int row = 0, tmp;
    double L, U, tmp2;
    for (int i = 0; i < n; i++)   P[i] = i;
    for (int i = 0; i < n - 1; i++) {
        tmp2 = 0.;
        for(int j = i; j < n; j++)
            if (std::abs(mA(j, i)) > tmp2) {
                tmp2 = std::abs(mA(j, i));
                row = j;
            }

        if (tmp2 == 0) {
            AfxMessageBox((CString)"矩阵奇异，无法计算");
            return FALSE;
        }

        tmp = P[i], P[i] = P[row], P[row] = tmp;

        for (int j = 0; j < n; j++)
            tmp2 = mA(i, j), mA(i, j) = mA(row, j), mA(row, j) = tmp2;

        U = mA(i, i), L = 0;
        for (int j = i + 1; j < n; j++) {
            L = mA(j, i) / U;
            mA(j, i) = L;
            for (int k = i + 1; k < n; k++)
                mA(j, k) = mA(j, k) - mA(i, k) * L;
        }
    }
    for (int i = 0; i < n; i++) {
        mL(i, i) = 1;
        for (int j = 0; j < i; j++)   mL(i, j) = mA(i, j);
        for (int j = i; j < n; j++)   mU(i, j) = mA(i, j);
    }
    return TRUE;
}

matrix LUPSolve(matrix& mL, matrix& mU, int* P, double* b) {
    int n = mL.rows();
    matrix mX(n, 1);
    double* y = new double[n]();

    for (int i = 0; i < n; i++) {
        y[i] = b[P[i]];
        for (int j = 0; j < i; j++)   y[i] -= mL(i, j) * y[j];
    }

    for (int i = n - 1; i >= 0; i--) {
        mX(i, 0) = y[i];
        for (int j = n - 1; j > i; j--)   mX(i, 0) -= mU(i, j) * mX(j, 0);
        mX(i, 0) /= mU(i, i);
    }

    return mX;
}

// Inverse
matrix matrix::I() {
    matrix mX(n, 1);
    matrix mL(n, m);
    matrix mU(n, m);
    matrix mMir(n, m);
    matrix mInv(n, m);
    int* P = new int[n]();
    double* b = new double[n]();

    if (n != m) {
        AfxMessageBox((CString)"无法计算非方阵");
        return mInv;
    }

    for (int i = 0; i < n; i++)
            for (int j = 0; j < m; j++) mMir.element[i][j] = element[i][j];
    LUPDescomposition(mMir, mL, mU, P);

    for (int i = 0; i < n; i++) {
        for (int j = 0; j < n; j++)   b[j] = 0;
        b[i] = 1;

        mX = LUPSolve(mL, mU, P, b);
        for (int j = 0; j < n; j++)   mInv(j, i) = mX(j, 0);
    }
    return mInv;
}

// Transposition
matrix matrix::T() {
    matrix w(m, n);
    for (int i = 0; i < m; i++)
        for (int j = 0; j < n; j++) w.element[i][j] = element[j][i];
    return w;
}

double Gaussian(double x, double A, double mean, double sigma) {
    return A * exp(-(x - mean) * (x - mean) / (2 * sigma * sigma));
}

matrix Jacobian(int n, double* X, double A, double mean, double sigma) {
    matrix J(n, 3);
    for (int i = 0; i < n; i++) {
        J(i, 0) = -Gaussian(X[i], A, mean, sigma) / A;
        J(i, 1) = -(X[i] - mean) * Gaussian(X[i], A, mean, sigma) / (sigma * sigma);
        J(i, 2) = -(X[i] - mean) * (X[i] - mean) * Gaussian(X[i], A, mean, sigma) / (sigma * sigma * sigma);
    }
    return J;
}

#endif