// simeq_newton3.swift  solve nonlinear system of equations
//                      method: newton iteration using Jacobian
//                      use list for higher order terms
//
//  Given problem  A X = Y  where X may have terms x1, x2, x3, x4,
//                 and higher order such as: x1*x2, x1*x1*x3, x4*x4*x4, ...
//                 A sparse matrix may be used, coefficients given
//                 Y is vector of reals given
//                 independent unknowns are x1, x2, x3, x4
//
//  for testing, generate A using pseudo random numbers
//               choose x1=1.1 x2=1.2 x3=1.4 x4 =1.5, compute products
//               compute terms of Y using Y = A X
//
//  Solve by initial guess at values of x1, x2, x3, x4 computing products
//    X_next = X_initial - J_initial^-1 * (A *  X_initial - Y)
//    in general X_next = X - (J_prev^-1 * (A * X - Y))*b
//                                 where 0 < b < 1, often 0.5, for stability
//
//  solved when  abs sum each row A * X_next -Y < epsilon
//
//  It may stall, stop if abs(X_next-X)<epsilon
//  It may diverge, stop, indicate no solution
//                        (or try a different initial guess)
//  It may oscillate, stop, indicate no solution
//                        (or try a different initial guess)
//
//
//  The matrix equation for this case is:
//
//   x1     x2     x3     x4     x1*x2 ... x3*x4  (may be subset of nl terms) 
//
// | A1,1   A1,2   A1,3   A1,4   A1,5 ...  A1,10  | | x1    | |y1 |
// | A2,1   A2,2   A2,3   A2,4   A2,5 ...  A2,10  | | x2    | |y2 |
// | A3,1   A3,2   A3,3   A3,4   A3,5 ...  A3,10  | | x3    | |y3 |
// | A4,1   A4,2   A4,3   A4,4   A4,5 ...  A4,10  |*| x4    |=|y4 |
// | A5,1   A5,2   A5,3   A5,4   A5,5 ...  A5,10  | | x1*x2 | |y5 |
// | A6,1   A6,2   A6,3   A6,4   A6,5 ...  A6,10  | | x1*x3 | |y6 |
// | ...                                          | |       | |   |
// | A10,1  A10,2  A10,3  A10,4  A10,5 ... A10,10 | | x3*x4 | |y10|
//
//  The Jacobian, J is the numerically computed derivatives based on A, X
//  The derivative coefficients may be computed once based on A and the
//  unknown variables. The numeric value plugs in the X values.
//
//  J1,1 = A1,1 + A1,5*x2 + A1,6*x3 + A1,7*x4   deriv Row 1 wrt x1
//  J1,2 = A1,2 + A1,5*x1 + A1,8*x3 + A1,9*x4   deriv Row 1 wrt x2
//  J1,3 = A1,3 + A1,6*x1 + A1,8*x2 + A1,10*x4  deriv Row 1 wrt x3
//  J1,4 = A1,4 + A1,7*x1 + A1,9*x2 + A1,10*x3  deriv Row 1 wrt x4
//  J1,5 = A1,5                                 deriv Row 1 wrt x1, wrt x2
//  J1,6 = A1,6                                 deriv Row 1 wrt x1, wrt x3
//  J1,7 = A1,7                                 deriv Row 1 wrt x1, wrt x4
//  J1,8 = A1,8                                 deriv Row 1 wrt x2, wrt x3
//  J1,9 = A1,9                                 deriv Row 1 wrt x2, wrt x4
//  J1,10 = A1,10                               deriv Row 1 wrt x3, wrt x4
//
//  etc.
//
//  j4,1 ... j4,10  should be last ones needed? others not independent
//
//  Code or a data structure must be available to know the equation
//  of the entries in the Y vector. Numeric computation of
//  derivatives is used, based on var1, var2, var3.
//
// derivatives not hand coded, computed from var1 and var2 and var3 to
// load Ja, initial guess in X[] n+nlin size
	
func simeq_newton3(_ n: Int, _ nlin: Int, _ A: [[Double]],
                   _ Y: [Double], _ X1: [Double],
		   _ var1: [Int], _ var2: [Int], _ var3: [Int],
		   _ uiter: Int, _ ub: Double, _ ueps: Double,
		   _ monitor: Int) -> [Double] { // X

  var eps = 1.0e-6 // possible default
  var b = 0.5 // stability convergence  factor  max 1.0
  var resid = 0.0  // residual from last iteration
  var presid = 0.0 // residual from prior to last iteration
  var maxiter = 10 // maximum number of iterations
  var Ja = [[Double]](repeating:[Double](repeating:0.0,count:n),count:n)
  var X       = [Double](repeating:0.0,count:n+nlin)
  var X_resid = [Double](repeating:0.0,count:n+nlin)
  var X_tmp2  = [Double](repeating:0.0,count:n+nlin)
  var X_next  = [Double](repeating:0.0,count:n+nlin)
  
  if monitor>0 { 
    print("simeq_newton3 running n=\(n), nlin=\(nlin)")
  } // end if
  eps = ueps
  maxiter = uiter
  b = ub
  // setup nonlinear terms
  for i in n..<n+nlin { // in nonlinear range
    X[i] = X[var1[i]] // must be valid
    if var2[i]>=0 {
      X[i] *= X[var2[i]] // -1 flag should not be used
    } // end if
    if var3[i]>=0 {
      X[i] *= X[var3[i]]
    } // end if
  } // end i
  for i in 0..<n+nlin {
    X[i] = X1[i]
    X_next[i] = X1[i]
  } // end i

  presid = 1.0e12
  // iterate to find solution
  for itr in 0..<maxiter {
    // compute residual
    resid = 0.0 
    for i in 0..<n {
      X_resid[i] = 0.0
      for j in 0..<n+nlin {
	X_resid[i] += A[i][j]*X[j]
      } // end j
      X_resid[i] -= Y[i] // X_resid used later if not solved
      resid = max(resid,abs(X_resid[i]))
    } // end i
    // debug print
    if monitor>2 {
      for i in 0..<n {
        print("residual X_resid[\(i)]=\(X_resid[i])")
      } // end i
      print(" ")
    } // end if

    // check for convergence
    if monitor>0 {
      print("simeq_newton3 itr \(itr), prev=\(presid) residual=\(resid)")
    } // end if
    if resid<eps {
      break
    } // end if
    // compute Jacobian
    for i in 0..<n { // equation
      for j in 0..<n { // variable
        Ja[i][j] = A[i][j] // each linear term
        for k in n..<n+nlin { // nonlinear
          // add derivative of nonlinear term
          if var1[k]==j && var2[k]==j && var3[k]==j {       /* X^3 */
            Ja[i][j] += A[i][k]*3.0*X[var2[k]]*X[var3[k]]   /* 3X^2 */
          } // end if
          else if var1[k]==j && var2[k]==j && var3[k]>=0 {  /* X^2 xa */
            Ja[i][j] += A[i][k]*2.0*X[var2[k]]*X[var3[k]]   /* 2X xa */
          } // end else if
          else if var1[k]==j && var2[k]==j && var3[k]<0 {    /* X^2 */
            Ja[i][j] += A[i][k]*2.0*X[var2[k]]               /* 2X */
          } // end else if
          else if var2[k]==j && var3[k]==j && var1[k]>=0 {   /* xa X^2 */
            Ja[i][j] += A[i][k]*2.0*X[var3[k]]*X[var1[k]]    /* 2X xa */
          } // end else if
          else if var2[k]==j && var3[k]==j && var1[k]<0 {    /*  X^2 */
            Ja[i][j] += A[i][k]*2.0*X[var3[k]]               /* 2X  */
          } // end else if
          else if var3[k]==j && var1[k]==j && var2[k]>=0 {   /* xa X^2 */
            Ja[i][j] += A[i][k]*2.0*X[var1[k]]*X[var2[k]]    /* 2X xa */
          } // end else if
          else if var3[k]==j && var1[k]==j && var2[k]<0 {    /*  X^2 */
            Ja[i][j] += A[i][k]*2.0*X[var1[k]]                /* 2X  */
          } // end else if
          else if var1[k]==j {
            if var2[k]>=0 && var3[k]>=0 { 
              Ja[i][j] += A[i][k]*X[var2[k]]*X[var3[k]]
            } // end if
            else if var2[k]>=0 {
	      Ja[i][j] += A[i][k]*X[var2[k]]
	    } // end else if
            else if var3[k]>=0 {
	      Ja[i][j] += A[i][k]*X[var3[k]]
	    } // end else if
	  } // end if var1
          else if var2[k]==j {
            if var1[k]>=0 && var3[k]>=0 { 
              Ja[i][j] += A[i][k]*X[var1[k]]*X[var3[k]]
	    } // end if
            else if var1[k]>=0 {
	      Ja[i][j] += A[i][k]*X[var1[k]]
	    } // end else if
            else if var3[k]>=0 {
	      Ja[i][j] += A[i][k]*X[var3[k]]
	    } // end else if
	  } // end if var2
          else if var3[k]==j {
            if var2[k]>=0 && var1[k]>=0 { 
              Ja[i][j] += A[i][k]*X[var2[k]]*X[var1[k]]
            } // end if
            else if var2[k]>=0 {
	      Ja[i][j] += A[i][k]*X[var2[k]]
	    } // else if
            else if var1[k]>=0 {
	      Ja[i][j] += A[i][k]*X[var1[k]]
	    } // end else if
          } // end if var3
        } // end k
      } // end j
    } // end i
    if monitor>4 {
      print("Ja computed ")
      for i in 0..<n {
        for j in 0..<n {
	  print("Ja[\(i)][\(j)]=\(Ja[i][j])")
	} // end j
      } // end i
      print(" ")
    } // end if

    // invert Ja
    Ja = matinv(Ja)
    if monitor>5 {
      print("Ja inverted ")
      for i in 0..<n {
        for j in 0..<n {
	  print("Ja[\(i)][\(j)]=\(Ja[i][j])")
	} // end j
      } // end i
      print(" ")
    } // end if

    // X_resid = A X - Y        error vector
    // X_tmp2 = J^-1 * X_resid  raw correction vector
    // X_next = X - (X_tmp2)*b weighted correction vector
    // X      = X_next         next X with computed higher terms

    for i in 0..<n {
      X_tmp2[i] = 0.0
      for j in 0..<n {
	X_tmp2[i] += Ja[i][j]*X_resid[j] 
      } // end j
    } // end i
    // debug print
    if monitor>3 {
      for i in 0..<n {
        print("change X_tmp2[\(i)]=\(X_tmp2[i])")
      } // end i
      print(" ")
    } // end if
    for i in 0..<n {
      X_next[i] = X[i] - b*X_tmp2[i]
    } // end i
    for i in n..<n+nlin {
      X_next[i] = X_next[var1[i]] // must be valid
      if var2[i]>=0 {
        X_next[i] *= X_next[var2[i]]
      } // end if
      if var3[i]>=0 {
         X_next[i] *= X_next[var3[i]]
      } // end if
    } // end i

    if monitor>2 {
      for i in 0..<n+nlin {
        print("X_next[\(i)]=\(X_next[i])")
      } // end i
      print(" ")
    } // end if

    // iterate
    presid = resid 
    for i in 0..<n+nlin {
      X[i] = X_next[i]
    } // end i
    if monitor>2 {
      print(" ")
    } // end if

  } // end iteration    

  if monitor>0 {
    print("simeq_newton3.swift finished")
  } // end if
  return X
} // end  simeq_newton3

// end simeq_newton3.swift


// matinv.swift  should have  import
func matinv(_ a: [[Double]]) -> [[Double]] {
  let n = a.count
  var inv = [[Double]](repeating:[Double](repeating:0.0,count:n),count:n)
  for i in 0..<n {
    for j in 0..<n {
      inv[i][j] = a[i][j]
    }
  }

  var row = [Int](repeating:0,count:n)
  var col = [Int](repeating:0,count:n)
  var temp = [Double](repeating:0,count:n)
  var hold = 0
  var I_pivot = 0
  var J_pivot = 0
  var pivot = 0.0
  var abs_pivot = 0.0

  // set up row and column interchange vectors
  for k in 0..<n {
    row[k] = k
    col[k] = k
  }
  // begin main reduction loop
  for k in 0..<n {
    // find largest element for pivot
    pivot = inv[row[k]][col[k]]
    I_pivot = k
    J_pivot = k
    for i in k..<n {
      for j in k..<n {
        abs_pivot = abs(pivot)
        if abs(inv[row[i]][col[j]]) > abs_pivot {
          I_pivot = i
          J_pivot = j
          pivot = inv[row[i]][col[j]]
        }
      }
    }
    if abs(pivot) < 1.0E-10 {
      print("Matrix is singular !")
      return [[Double]](repeating:[Double](repeating:0.0,count:n),count:n)
    }
    hold = row[k]
    row[k] = row[I_pivot]
    row[I_pivot] = hold
    hold = col[k]
    col[k] = col[J_pivot]
    col[J_pivot] = hold
    // reduce about pivot
    inv[row[k]][col[k]] = 1.0 / pivot
    for j in 0..<n {
      if j != k {
        inv[row[k]][col[j]] = inv[row[k]][col[j]] * inv[row[k]][col[k]]
      }
    }
    // inner reduction loop
    for i in 0..<n {
      if k != i {
        for j in 0..<n {
          if k != j {
            inv[row[i]][col[j]] = inv[row[i]][col[j]] - inv[row[i]][col[k]] * inv[row[k]][col[j]]
          }
        }
        inv[row[i]][col [k]] = -inv[row[i]][col[k]] * inv[row[k]][col[k]]
      }
    }
  }
  // end main reduction loop

  // unscramble rows
  for j in 0..<n {
    for i in 0..<n {
      temp[col[i]] = inv[row[i]][j]
    }
    for i in 0..<n {
      inv[i][j] = temp[i]
    }
  }
  // unscramble columns
  for i in 0..<n {
    for j in 0..<n {
      temp[row[j]] = inv[i][col[j]]
    }
    for j in 0..<n {
      inv[i][j] = temp[j]
    }
  }
  return inv
} // end matinv