extern "C" {
#include <assert.h>
#include <stdlib.h>
}
#include <poincare/matrix.h>
#include <poincare/complex.h>
#include <poincare/addition.h>
#include <poincare/decimal.h>
#include <poincare/undefined.h>
#include "layout/grid_layout.h"
#include "layout/bracket_layout.h"
#include <cmath>
#include <float.h>
#include <string.h>

namespace Poincare {

Matrix::Matrix(MatrixData * matrixData) :
  DynamicHierarchy()
{
  assert(matrixData != nullptr);
  m_numberOfOperands = matrixData->numberOfRows()*matrixData->numberOfColumns();
  m_numberOfRows = matrixData->numberOfRows();
  matrixData->pilferOperands(&m_operands);
  for (int i = 0; i < m_numberOfOperands; i++) {
    const_cast<Expression *>(m_operands[i])->setParent(this);
  }
}

Matrix::Matrix(const Expression * const * operands, int numberOfRows, int numberOfColumns, bool cloneOperands) :
  DynamicHierarchy(operands, numberOfRows*numberOfColumns, cloneOperands),
  m_numberOfRows(numberOfRows)
{
}

int Matrix::numberOfRows() const {
  return m_numberOfRows;
}

int Matrix::numberOfColumns() const {
  return numberOfOperands()/m_numberOfRows;
}

Expression::Type Matrix::type() const {
  return Type::Matrix;
}

Expression * Matrix::clone() const {
  return new Matrix(m_operands, numberOfRows(), numberOfColumns(), true);
}

int Matrix::writeTextInBuffer(char * buffer, int bufferSize, int numberOfSignificantDigits) const {
  if (bufferSize == 0) {
    return -1;
  }
  buffer[bufferSize-1] = 0;
  int currentChar = 0;
  if (currentChar >= bufferSize-1) {
    return 0;
  }
  buffer[currentChar++] = '[';
  if (currentChar >= bufferSize-1) {
    return currentChar;
  }
  for (int i = 0; i < numberOfRows(); i++) {
    buffer[currentChar++] = '[';
    if (currentChar >= bufferSize-1) {
      return currentChar;
    }
    currentChar += operand(i*numberOfColumns())->writeTextInBuffer(buffer+currentChar, bufferSize-currentChar, numberOfSignificantDigits);
    if (currentChar >= bufferSize-1) {
      return currentChar;
    }
    for (int j = 1; j < numberOfColumns(); j++) {
      buffer[currentChar++] = ',';
      if (currentChar >= bufferSize-1) {
        return currentChar;
      }
      currentChar += operand(i*numberOfColumns()+j)->writeTextInBuffer(buffer+currentChar, bufferSize-currentChar, numberOfSignificantDigits);
      if (currentChar >= bufferSize-1) {
        return currentChar;
      }
    }
    currentChar = strlen(buffer);
    if (currentChar >= bufferSize-1) {
      return currentChar;
    }
    buffer[currentChar++] = ']';
    if (currentChar >= bufferSize-1) {
      return currentChar;
    }
  }
  buffer[currentChar++] = ']';
  buffer[currentChar] = 0;
  return currentChar;
}

ExpressionLayout * Matrix::privateCreateLayout(FloatDisplayMode floatDisplayMode, ComplexFormat complexFormat) const {
  assert(floatDisplayMode != FloatDisplayMode::Default);
  assert(complexFormat != ComplexFormat::Default);
  ExpressionLayout ** childrenLayouts = new ExpressionLayout * [numberOfOperands()];
  for (int i = 0; i < numberOfOperands(); i++) {
    childrenLayouts[i] = operand(i)->createLayout(floatDisplayMode, complexFormat);
  }
  ExpressionLayout * layout = new BracketLayout(new GridLayout(childrenLayouts, numberOfRows(), numberOfColumns()));
  delete [] childrenLayouts;
  return layout;
}

template<typename T>
Complex<T> * Matrix::createTrace() const {
  if (numberOfRows() != numberOfColumns()) {
    return new Complex<T>(Complex<T>::Float(NAN));
  }
  int dim = numberOfRows();
  Complex<T> c = Complex<T>::Float(0);
  for (int i = 0; i < dim; i++) {
    assert(operand(i*dim+i)->type() == Type::Complex);
    c = Addition::compute(c, *(static_cast<const Complex<T> *>(operand(i*dim+i))));
  }
  return new Complex<T>(c);
}

// TODO: 1. implement determinant/inverse for complex matrix
// TODO: 2. implement determinant/inverse for any expression (do not evaluate first)
template<typename T>
Complex<T> * Matrix::createDeterminant() const {
  if (numberOfRows() != numberOfColumns()) {
    return new Complex<T>(Complex<T>::Float(NAN));
  }
  int dim = numberOfRows();
  T ** tempMat = new T*[dim];
  for (int i = 0; i < dim; i++) {
    tempMat[i] = new T[dim];
  }
  T det = 1;
  /* Copy the matrix */
  for (int i = 0; i < dim; i++) {
    for (int j = 0; j < dim; j++) {
      const Expression * op = operand(i*dim+j);
      assert(op->type() == Type::Complex);
      tempMat[i][j] = static_cast<const Complex<T> *>(op)->toScalar(); // TODO: keep complex
    }
  }

  /* Main Loop: Gauss pivot */
  for (int i = 0; i < dim-1; i++) {
    /* Search for pivot */
    int rowWithPivot = i;
    for (int row = i+1; row < dim; row++) {
      if (std::fabs(tempMat[rowWithPivot][i]) < std::fabs(tempMat[row][i])) {
        rowWithPivot = row;
      }
    }
    T valuePivot = tempMat[rowWithPivot][i];
    /* if the pivot is null, det = 0. */
    if (std::fabs(valuePivot) <= (sizeof(T) == sizeof(float) ? FLT_EPSILON : DBL_EPSILON)) {
      for (int i = 0; i < dim; i++) {
        delete[] tempMat[i];
      }
      delete[] tempMat;
      return new Complex<T>(Complex<T>::Float(0.0));
    }
    /* Switch rows to have the pivot row as first row */
    if (rowWithPivot != i) {
      for (int col = i; col < dim; col++) {
        T temp = tempMat[i][col];
        tempMat[i][col] = tempMat[rowWithPivot][col];
        tempMat[rowWithPivot][col] = temp;
      }
      det *= -1;
    }
    det *= valuePivot;
    /* Set to 0 all A[][i] by linear combination */
    for (int row = i+1; row < dim; row++) {
      T factor = tempMat[row][i]/valuePivot;
      for (int col = i; col < dim; col++) {
        tempMat[row][col] -= factor*tempMat[i][col];
      }
    }
  }
  det *= tempMat[dim-1][dim-1];
  for (int i = 0; i < dim; i++) {
    delete[] tempMat[i];
  }
  delete[] tempMat;
  return new Complex<T>(Complex<T>::Float(det));
}

template<typename T>
Matrix * Matrix::createInverse() const {
  if (numberOfRows() != numberOfColumns()) {
    return nullptr;
  }
  int dim = numberOfRows();
  /* Create the matrix inv = (A|I) with A the input matrix and I the dim identity matrix */
  T ** inv = new T*[dim];
  for (int i = 0; i < dim; i++) {
    inv[i] = new T [2*dim];
  }
  for (int i = 0; i < dim; i++) {
    for (int j = 0; j < dim; j++) {
      const Expression * op = operand(i*dim+j);
      assert(op->type() == Type::Complex);
      inv[i][j] = static_cast<const Complex<T> *>(op)->toScalar(); // TODO: keep complex
    }
    for (int j = dim; j < 2*dim; j++) {
      inv[i][j] = (i+dim == j);
    }
  }
  /* Main Loop: Gauss pivot */
  for (int i = 0; i < dim; i++) {
    /* Search for pivot */
    int rowWithPivot = i;
    for (int row = i+1; row < dim; row++) {
      if (std::fabs(inv[rowWithPivot][i]) < std::fabs(inv[row][i])) {
        rowWithPivot = row;
      }
    }
    T valuePivot = inv[rowWithPivot][i];
    /* if the pivot is null, the matrix in not invertible. */
    if (std::fabs(valuePivot) <= (sizeof(T) == sizeof(float) ? FLT_EPSILON : DBL_EPSILON)) {
      for (int i = 0; i < dim; i++) {
        delete[] inv[i];
      }
      delete[] inv;
      return nullptr;
    }
    /* Switch rows to have the pivot row as first row */
    if (rowWithPivot != i) {
      for (int col = i; col < 2*dim; col++) {
        T temp = inv[i][col];
        inv[i][col] = inv[rowWithPivot][col];
        inv[rowWithPivot][col] = temp;
      }
    }
    /* A[pivot][] = A[pivot][]/valuePivot */
    for (int col = 0; col < 2*dim; col++) {
      inv[i][col] /= valuePivot;
    }
    /* Set to 0 all A[][row] by linear combination */
    for (int row = 0; row < dim; row++) {
      if (row == i) {
        continue;
      }
      T factor = inv[row][i];
      for (int col = 0; col < 2*dim; col++) {
        inv[row][col] -= factor*inv[i][col];
      }
    }
  }
  const Expression ** operands = new const Expression * [numberOfOperands()];
  for (int i = 0; i < dim; i++) {
    for (int j = 0; j < dim; j++) {
      operands[i*dim+j] = new Complex<T>(Complex<T>::Float(inv[i][j+dim]));
    }
  }
  for (int i = 0; i < dim; i++) {
    delete[] inv[i];
  }
  delete[] inv;
  // Intentionally swapping dimensions for inverse, although it doesn't make a difference because it is square
  Matrix * matrix = new Matrix(operands, numberOfColumns(), numberOfRows(), false);
  delete[] operands;
  return matrix;
}

Matrix * Matrix::createTranspose() const {
  const Expression ** operands = new const Expression * [numberOfOperands()];
  for (int i = 0; i < numberOfRows(); i++) {
    for (int j = 0; j < numberOfColumns(); j++) {
      operands[j*numberOfRows()+i] = operand(i*numberOfColumns()+j);
    }
  }
  // Intentionally swapping dimensions for transpose
  Matrix * matrix = new Matrix(operands, numberOfColumns(), numberOfRows(), true);
  delete[] operands;
  return matrix;
}

Matrix * Matrix::createIdentity(int dim) {
  Expression ** operands = new Expression * [dim*dim];
  for (int i = 0; i < dim; i++) {
    for (int j = 0; j < dim; j++) {
      if (i == j) {
        operands[i*dim+j] = new Rational(1);
      } else {
        operands[i*dim+j] = new Rational(0);
      }
    }
  }
  Matrix * matrix = new Matrix(operands, dim, dim, false);
  delete [] operands;
  return matrix;
}

template<typename T>
Matrix * Matrix::createApproximateIdentity(int dim) {
  Expression ** operands = new Expression * [dim*dim];
  for (int i = 0; i < dim; i++) {
    for (int j = 0; j < dim; j++) {
      if (i == j) {
        operands[i*dim+j] = new Complex<T>(Complex<T>::Float(1));
      } else {
        operands[i*dim+j] = new Complex<T>(Complex<T>::Float(0));
      }
    }
  }
  Matrix * matrix = new Matrix(operands, dim, dim, false);
  delete [] operands;
  return matrix;
}

template<typename T>
Expression * Matrix::templatedApproximate(Context& context, AngleUnit angleUnit) const {
  Expression ** operands = new Expression * [numberOfOperands()];
  for (int i = 0; i < numberOfOperands(); i++) {
    Expression * operandEvaluation = operand(i)->approximate<T>(context, angleUnit);
    if (operandEvaluation->type() != Type::Complex) {
      operands[i] = new Complex<T>(Complex<T>::Float(NAN));
      delete operandEvaluation;
    } else {
      operands[i] = operandEvaluation;
    }
  }
  Expression * matrix = new Matrix(operands, numberOfRows(), numberOfColumns(), false);
  delete[] operands;
  return matrix;
}

template Matrix* Matrix::createApproximateIdentity<float>(int);
template Matrix* Matrix::createApproximateIdentity<double>(int);
template Complex<float>* Matrix::createTrace<float>() const;
template Complex<double>* Matrix::createTrace<double>() const;
template Matrix* Matrix::createInverse<float>() const;
template Matrix* Matrix::createInverse<double>() const;
template Complex<float>* Matrix::createDeterminant<float>() const;
template Complex<double>* Matrix::createDeterminant<double>() const;
}