// 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  simeq.swift