// test_simeq.swift  solve  A X = Y  given matrix A and vector Y

#if os(OSX) || os(iOS) // for libraries, portable for OSX and Linux
import Foundation  // or Playground
#elseif os(Linux)
import Glibc
#endif 

print("test_simeq.swift running  simeq")
var A = [[1.0, 2.0, 3.0], [2.0, 4.0, 4.0], [3.0, -2.0, -1.0]]
print("A = \(A) \n")
var X = [2.0, 5.0, -3.0]
print("X = \(X) \n")
var Y = matvecmul(A, X)
print("Y = \(Y) \n")
var Xr = [0.0, 0.0, 0.0]
Xr = simeq(A, Y)
print("Xr = simeq(A, Y) =\(Xr) \n")
print("check Xr about X with roundoff")

print("\ntest_simeq.swift running  simeqb")
let n = 3
let m = 4
var AY = [[Double]](repeating:[Double](repeating:0.0,count:m),count:n)
for i in 0..<n {
  for j in 0..<n {
    AY[i][j] = A[i][j]
  }
  AY[i][n] = Y[i]
}
print("AY = \(AY) \n")
print("X = \(X) \n")
Xr = simeqb(AY)
print("Xr = simeqb(AY) = \(Xr) \n")
print("check Xr about X with roundoff")

print("\ntest_simeq.swift running  simeq2")
var A2 = [[1.0, 2.0, 3.0], [2.0, 4.0, 4.0], [3.0, -2.0, -1.0]]
print("A2 = \(A2) \n")
var X2 = [2.0, 5.0, -3.0]
print("X2 = \(X2) \n")
var Y2 = matvecmul(A2, X2)
print("Y2 = \(Y2) \n")
var Xr2 = [0.0, 0.0, 0.0]
simeq2(3, A2, &Xr2, Y2)
print("Xr2 = \(Xr2) \n")
print("check Xr2 about X2 with roundoff \n")

print("\ntest_simeq.swift finished")

// simeq.swift  solve  A X = Y  given matrix A and vector Y
//                     X is solution
//    WRITTEN BY : JON SQUIRE , 3 Dec 2017
//    ORIGONAL DEC 1959 for IBM 650, TRANSLATED TO OTHER LANGUAGES
//    e.g. FORTRAN converted to Ada converted to C and many other languages
//    X[] = simeq(A[][],Y[])
//    X[] = simeqb(AY[][])    Y as last column of A
//    X[] = simeq2(n,A[][],X[],Y[])

func simeq(_ A: [[Double]], _ Y: [Double]) -> [Double] { // X 
  let n = A.count
  let m = n+1
  var X = [Double](repeating:0.0,count:n)
  var B = [[Double]](repeating:[Double](repeating:0.0,count:m),count:n)
  var ROW = [Int](repeating:0,count:n) // ROW INTERCHANGE INDICIES
  var HOLD = 0
  var I_PIVOT = 0      // PIVOT INDICIES
  var PIVOT = 0.0      // PIVOT ELEMENT VALUE
  var ABS_PIVOT = 0.0

  // check sizes
  if n != A[0].count || n != Y.count {
    print("inconistant size arrays rows=\(n),cols=\(A[0].count),Y=\(Y.count)")
    return X
  }
  // BUILD WORKING DATA STRUCTURE 
  for i in 0..<n {
    for j in 0..<n {
      B[i][j] = A[i][j]
    }
    B[i][n] = Y[i]
  }

  // SET UP ROW  INTERCHANGE VECTORS
  for k in 0..<n {
    ROW[k] = k
  }

  // BEGIN MAIN REDUCTION LOOP 
  for k in 0..<n {
    // FIND LARGEST ELEMENT FOR PIVOT 
    PIVOT = B[ROW[k]][k]
    ABS_PIVOT = abs(PIVOT)
    I_PIVOT = k
    for i in k..<n {
      if abs(B[ROW[i]][k]) > ABS_PIVOT {
        I_PIVOT = i
        PIVOT = B[ROW[i]][k]
        ABS_PIVOT = abs(PIVOT)
      }
    }

    // HAVE PIVOT, INTERCHANGE ROW POINTERS
    HOLD = ROW[k]
    ROW[k] = ROW[I_PIVOT]
    ROW[I_PIVOT] = HOLD

    // CHECK FOR NEAR SINGULAR
    if ABS_PIVOT < 1.0E-32 {
      for j in k..<m {
        B[ROW[k]][j] = 0.0
      }
      print("redundant row (singular) \(ROW[k])")
    } // singular, delete row
    else {

      // REDUCE ABOUT PIVOT 
      for j in k+1..<m {
        B[ROW[k]][j] = B[ROW[k]][j] / B[ROW[k]][k]
      }

      // INNER REDUCTION LOOP 
      for i in 0..<n {
        if i != k {
          for j in k+1..<m {
            B[ROW[i]][j] = B[ROW[i]][j] - B[ROW[i]][k] * B[ROW[k]][j]
          }
        }
      }
    }
    // FINISHED INNER REDUCTION 
  }

  // END OF MAIN REDUCTION LOOP 
  // BUILD  X  FOR RETURN, UNSCRAMBLING ROWS 
  for i in 0..<n {
    X[i] = B[ROW[i]][n]
  }
  return X
} // end simeq

func simeqb(_ AY: [[Double]]) -> [Double] { // X 
  let n = AY.count
  let m = n+1
  var X = [Double](repeating:0.0,count:n)
  var B = [[Double]](repeating:[Double](repeating:0.0,count:m),count:n)
  var ROW = [Int](repeating:0,count:n) // ROW INTERCHANGE INDICIES
  var HOLD = 0
  var I_PIVOT = 0      // PIVOT INDICIES
  var PIVOT = 0.0      // PIVOT ELEMENT VALUE
  var ABS_PIVOT = 0.0

  // check sizes
  if m != AY[0].count {
    print("inconistant size arrays rows=\(n),cols=\(AY[0].count)")
    return X
  }
  // BUILD WORKING DATA STRUCTURE 
  for i in 0..<n {
    for j in 0..<m {
      B[i][j] = AY[i][j]
    }
  }

  // SET UP ROW  INTERCHANGE VECTORS
  for k in 0..<n {
    ROW[k] = k
  }

  // BEGIN MAIN REDUCTION LOOP 
  for k in 0..<n {
    // FIND LARGEST ELEMENT FOR PIVOT 
    PIVOT = B[ROW[k]][k]
    ABS_PIVOT = abs(PIVOT)
    I_PIVOT = k
    for i in k..<n {
      if abs(B[ROW[i]][k]) > ABS_PIVOT {
        I_PIVOT = i
        PIVOT = B[ROW[i]][k]
        ABS_PIVOT = abs(PIVOT)
      }
    }

    // HAVE PIVOT, INTERCHANGE ROW POINTERS
    HOLD = ROW[k]
    ROW[k] = ROW[I_PIVOT]
    ROW[I_PIVOT] = HOLD

    // CHECK FOR NEAR SINGULAR
    if ABS_PIVOT < 1.0E-32 {
      for j in k..<m {
        B[ROW[k]][j] = 0.0
      }
      print("redundant row (singular) \(ROW[k])")
    } // singular, delete row
    else {

      // REDUCE ABOUT PIVOT 
      for j in k+1..<m {
        B[ROW[k]][j] = B[ROW[k]][j] / B[ROW[k]][k]
      }

      // INNER REDUCTION LOOP 
      for i in 0..<n {
        if i != k {
          for j in k+1..<m {
            B[ROW[i]][j] = B[ROW[i]][j] - B[ROW[i]][k] * B[ROW[k]][j]
          }
        }
      }
    }
    // FINISHED INNER REDUCTION 
  }

  // END OF MAIN REDUCTION LOOP 
  // BUILD  X  FOR RETURN, UNSCRAMBLING ROWS 
  for i in 0..<n {
    X[i] = B[ROW[i]][n]
  }
  return X
} // end simeqb

// simeq2   USAGE  :     simeq2(n,A,X,Y)
func simeq2(_ n: Int,_ A: [[Double]], _ X: inout [Double],_ Y: [Double]) {
  let m = n+1
  var B = [[Double]](repeating:[Double](repeating:0.0,count:m),count:n)
  var ROW = [Int](repeating:0,count:n) // ROW INTERCHANGE INDICIES
  var HOLD = 0
  var I_PIVOT = 0      // PIVOT INDICIES
  var PIVOT = 0.0      // PIVOT ELEMENT VALUE
  var ABS_PIVOT = 0.0
  // BUILD WORKING DATA STRUCTURE 
  for i in 0..<n {
    for j in 0..<n {
      B[i][j] = A[i][j]
    }
    B[i][n] = Y[i]
  }

  // SET UP ROW  INTERCHANGE VECTORS 
  for k in 0..<n {
    ROW[k] = k
  }

  // BEGIN MAIN REDUCTION LOOP 
  for k in 0..<n {

    // FIND LARGEST ELEMENT FOR PIVOT 
    PIVOT = B[ROW[k]][k]
    ABS_PIVOT = abs(PIVOT)
    I_PIVOT = k
    for i in k..<n {
      if(abs(B[ROW[i]][k]) > ABS_PIVOT) {
        I_PIVOT = i
        PIVOT = B[ROW[i]][k]
        ABS_PIVOT = abs(PIVOT)
      }
    }

    // HAVE PIVOT, INTERCHANGE ROW POINTERS 
    HOLD = ROW[k]
    ROW[k] = ROW[I_PIVOT]
    ROW[I_PIVOT] = HOLD

    // CHECK FOR NEAR SINGULAR 
    if  ABS_PIVOT < 1.0E-32 {
      for j in k+1..<n+1 {
        B[ROW[k]][j] = 0.0
      }
      print("redundant row (singular) \(ROW[k]) \n")
    } // singular, delete row 
    else {

      // REDUCE ABOUT PIVOT 
      for j in k+1..<n+1 {
        B[ROW[k]][j] = B[ROW[k]][j] / B[ROW[k]][k]
      }

      // INNER REDUCTION LOOP 
      for i in 0..<n {
        if i != k {
          for j in k+1..<n+1 {
            B[ROW[i]][j] = B[ROW[i]][j] - B[ROW[i]][k] * B[ROW[k]][j]
          }
        }
      }
    }
    // FINISHED INNER REDUCTION 
  }

  // END OF MAIN REDUCTION LOOP 
  // BUILD  X  FOR RETURN, UNSCRAMBLING ROWS 
  for i in 0..<n {
    X[i] = B[ROW[i]][n]
  }
}    // end simeq2

func matvecmul(_ a: [[Double]], _ v: [Double]) -> [Double] {
  let n	= a.count
  var prod = [Double](repeating:0.0,count:n)
  for i in 0..<n {
    for j in 0..<n {
      prod[i] = prod[i] + a[i][j]*v[j]
    }
  }
  return prod
} // end matvecmul

// end  test_simeq.swift