from grid import exact_func
import numpy as np
import sys

def get_phis(X, r):
  """
    The get_phis function is used to get barycentric coordonites for a point on a triangle.

    X -- the destination point (2D)
         X = [0,0]
    r -- the three points that make up the triangle (2D)
         r = [[-1, -1], [0, 2], [1, -1]]

    this will return [0.333, 0.333, 0.333]
  """

  # baker: eq 7
  A = np.array([
                [1,   1,   1  ],
                [r[0][0], r[1][0], r[2][0]],
                [r[0][1], r[1][1], r[2][1]],
                ])
  b = np.array([1, X[0], X[1]])
  try:
    phi = np.linalg.solve(A,b)
  except:
    print >> sys.stderr, "warning: calculation of phis yielded a linearly dependant system"
    phi = np.dot(np.linalg.pinv(A), b)

  return phi

def qlinear(X, r, q):
  """
    this calculates the linear portion of q from X to r

    X = destination point
    r = simplex points
    q = CFD quantities of interest at the simplex points
  """

  phis = get_phis(X, r)
  qlin = sum([q_i * phi_i for q_i, phi_i in zip(q[:len(phis)], phis)])
  return qlin

def run_baker(X, g, tree, extra_points = 3, verbose = False):
  """
    This is the main function to call to get an interpolation to X from the tree

    X -- the destination point (2D)
         X = [0,0]

    g -- the grid object

    tree -- the kdtree search object (built from the g mesh)
  """

  (dist, indicies) = tree.query(X, 3 + extra_points)

  nn = [g.points[i] for i in indicies]
  nq = [g.q[i]      for i in indicies]

  phi = get_phis(X, nn[:3])
  qlin =  nq[0] * phi[0] + nq[1] * phi[1] + nq[2] * phi[2]

  error_term = 0.0

  if extra_points != 0:
    B = [] # baker eq 9
    w = [] # baker eq 11

    for index in indicies[3:]:
      (phi1,phi2,phi3) = get_phis(g.points[index], nn)
      B.append([phi1 * phi2, phi2*phi3, phi3*phi1])

      w.append(g.q[index] - qlinear(g.points[index], nn, nq))

    B = np.array(B)
    w = np.array(w)

    A = np.dot(B.T, B)
    b = np.dot(B.T, w)

    # baker solve eq 10
    try:
      (a, b, c) = np.linalg.solve(A,b)
    except:
      print >> sys.stderr, "warning: linear calculation went bad, resorting to np.linalg.pinv"
      (a, b, c) = np.dot(np.linalg.pinv(A), b)

    error_term =  a * phi[0] * phi[1]\
                + b * phi[1] * phi[2]\
                + c * phi[2] * phi[0]

  exact = exact_func(X[0], X[1])
  q_final = qlin + error_term

  if verbose:
    print "current point : %s" % X
    print "exact         : %0.4f" % exact
    print "qlin          : %0.4f" % qlin
    print "qlinerr       : %0.4f" % np.abs(exact - qlin)
    print "q_final       : %0.4f" % q_final
    print "q_final_err   : %0.4f" % np.abs(exact - q_final)
    print

  return (q_final, exact)