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 triangular simplex (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 get_phis_3D(X, r):
  """
    The get_phis function is used to get barycentric coordonites for a point on a triangle.

    X -- the destination point (3D)
         X = [0,0,0]
    r -- the four points that make up the tetrahedron (3D)
         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,       1 ],
                [r[0][0], r[1][0], r[2][0], r[3][0]],
                [r[0][1], r[1][1], r[2][1], r[3][1]],
                [r[0][2], r[1][2], r[2][2], r[3][2]],
                ])
  b = np.array([    1,
                  X[0],
                  X[1],
                  X[2]
              ])
  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
    
    also, this is baker eq 3

    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, phis)])
  return qlin

def qlinear_3D(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(R)
  """

  phis = get_phis_3D(X, R)
  qlin = sum([q_i * phi_i for q_i, phi_i in zip(q, phis)])
  return qlin

def run_baker(X, R, S, 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)

  """

  # calculate values only for the triangle
  phi  = get_phis(X, S.points)
  qlin = qlinear (X, S.points, S.q)

  if extra_points == 0: return qlin

  B = [] # baker eq 9
  w = [] # baker eq 11

  for (s, q) in zip(S.points, S.q):
    (phi1, phi2, phi3) = get_phis(s, R.points)
    B.append([phi1 * phi2, phi2*phi3, phi3*phi1])

    w.append(q - qlinear(s, R.points, R.q))

  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]

  q_final = qlin + error_term

  answer = {
             'a': a,
             'b': b,
             'c': c,
             'qlin': qlin,
             'error': error_term,
             'final': q_final,
  }

  return answer