package simpleMath.linealAlgebra.solvers

import simpleMath.SimpleProfiler
import simpleMath.linealAlgebra.{MatrixD, VectorD}


object LUPDenseSolver {
  def apply(m: MatrixD) = new LUPDenseSolver(m)

  def main(args: Array[String]) {
    val m: MatrixD = MatrixD(6, 6)

    for(i <- 0 until m.rows){
      m(i,i) = 1
      m(i, m.rows - 1 - i) = i
    }
    val solver = LUPDenseSolver(m)
    SimpleProfiler.nanoSecs(LUPDenseSolver(m))
    println(s"matrix m:$m")
    println(s"LUP decomposition of m is:\n${solver.C}")
    val X = VectorD((0 until m.rows).toArray)
    val B = m * X
    println(s"B:\n$B")
    println(s"\nA.solve:\n${solver.solve(B)}")
    SimpleProfiler.nanoSecs(solver.solve(B))
    println(s"\nP:")
    solver.P.foreach(println)
  }
}

class LUPDenseSolver(m: MatrixD) extends linearSolver {
  if (m.rows != m.cols)
    throw new IllegalArgumentException(s"for solver rows and colums size should ne equals, but we have (${m.rows}," +
      s" ${m.cols}})")
  var C = m.clone()
  val N = m.rows
  val P = (0 until N).toArray //MatrixD.identity(N)

  for (i <- 0 until N) {
    var pivotValue = C(i, i)
    var pivot = i

    for (row <- i until N;
         temp = C(row, i).abs
         if temp > pivot) {
      pivotValue = temp
      pivot = row
    }

    if (pivotValue == 0.0)
      throw new IllegalArgumentException(s"Matrix is singular C($i, $i) == 0.0")


    //меняем местами i-ю строку и строку с опорным элементом
    if (pivot != i) {

      val temp_i = P(i)
      P(i) = P(pivot)
      P(pivot) = temp_i

      C.swapRows(pivot, i)
    }

    var j = i + 1
    while (j < N) {
      C(j, i) /= pivotValue
      var k = i + 1
      while (k < N) {
        C(j, k) -= C(j, i) * C(i, k)
        k += 1
      }
      j += 1
    }
  }

  override def solve(B: VectorD): VectorD = {
    val X = VectorD(N)
    for (i <- 0 until N)
      X(i) = B(P(i))
    solveUpperTriangular(solveLowTriangular(X))
  }

  private def solveLowTriangular(B: VectorD): VectorD = {
    for (i <- 1 until N; j <- 0 until i)
      B(i) -= B(j) * C(i, j)
    B
  }

  private def solveUpperTriangular(B: VectorD): VectorD = {
    var i = N - 1
    while (i >= 0) {
      var j = N - 1
      while (j > i) {
        B(i) -= B(j) * C(i, j)
        j -= 1
      }
      B(i) /= C(i, j)
      i -= 1
    }
    B
  }

}