from baker import *
from baker.tools import smblog
import numpy as np
import sys

from tools import smberror

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 containing 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 np.linalg.LinAlgError as e:
    msg = "calculation of phis yielded a linearly dependant system (%s)" % e
    smblog.error(msg)
    raise smberror(msg)
    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 tetrahedron.

    X -- the destination point (3D)
         X = [0,0,0]
    R -- the four points that make up the containing simplex, tetrahedron (3D)
         R = [
               [0.0, 0.0, 1.0],
               [0.94280904333606508, 0.0, -0.3333333283722672],
               [-0.47140452166803232, 0.81649658244673617, -0.3333333283722672],
               [-0.47140452166803298, -0.81649658244673584, -0.3333333283722672],
             ]

    this (should) will return [0.25, 0.25, 0.25, 0.25]
  """

  # 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 np.linalg.LinAlgError as e:
    smblog.error("calculation of phis yielded a linearly dependant system: %s" % e)
    phi = np.dot(np.linalg.pinv(A), b)

  return phi


def qlinear(X, R):
  """
    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.points)
  qlin = sum([q_i * phi_i for q_i, phi_i in zip(R.q, phis)])
  return phis, qlin

def qlinear_3D(X, R):
  """
    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.points)
  qlin = sum([q_i * phi_i for q_i, phi_i in zip(R.q, phis)])
  return phis, qlin

def get_error_quadratic(phi, R, S):
  B = [] # baker eq 9
  w = [] # baker eq 11

  for (s, q) in zip(S.points, S.q):
    cur_phi, cur_qlin = qlinear(s, R)
    (phi1, phi2, phi3) = cur_phi

    B.append(
              [
                phi1 * phi2,
                phi2 * phi3,
                phi3 * phi1,
              ]
            )
    w.append(q - cur_qlin)

  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 np.linalg.LinAlgError as e:
    smblog.error("linear calculation went bad, resorting to np.linalg.pinv: %s" % e)
    (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]

  return error_term

def get_error_cubic(phi, R, S):
  B = [] # baker eq 9
  w = [] # baker eq 11

  for (s, q) in zip(S.points, S.q):
    cur_phi, cur_qlin = qlinear(s, R)
    (phi1, phi2, phi3) = cur_phi

    # basing this on eq 17
    B.append(
              [
                phi1 * phi2 * phi2, # a
                phi1 * phi3 * phi3, # b
                phi2 * phi1 * phi1, # c
                phi2 * phi3 * phi3, # d
                phi3 * phi1 * phi1, # e
                phi3 * phi2 * phi2, # f
                phi1 * phi2 * phi3, # g
              ]
            )
    w.append(q - cur_qlin)

  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, d, e, f, g) = np.linalg.solve(A,b)
  except np.linalg.LinAlgError as e:
    smblog.error("linear calculation went bad, resorting to np.linalg.pinv: %s" % e)
    (a, b, c, d, e, f, g) = np.dot(np.linalg.pinv(A), b)

  error_term =  a * phi[0] * phi[1] * phi[1]\
              + b * phi[0] * phi[2] * phi[2]\
              + c * phi[1] * phi[0] * phi[0]\
              + d * phi[1] * phi[2] * phi[2]\
              + e * phi[2] * phi[0] * phi[0]\
              + f * phi[2] * phi[1] * phi[1]\
              + g * phi[0] * phi[1] * phi[2]\

  return error_term


def run_baker(X, R, S, order=2):
  """
    This is the main function to call to get an interpolation to X from the input meshes

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

    R = Simplex
    S = extra points
  """

  answer = {
             'qlin': None,
             'error': None,
             'final': None,
            }
  # calculate values only for the simplex triangle
  phi, qlin = qlinear(X, R)

  if order == 1:
    answer['qlin'] = qlin
    return answer
  elif order == 2:
    error_term = get_error_quadratic(phi, R, S)
  elif order == 3:
    error_term = get_error_cubic(phi, R, S)
  else:
    raise smberror('unacceptable order for baker method')

  q_final = qlin + error_term

  answer['qlin' ] =  qlin
  answer['error'] =  error_term
  answer['final'] =  q_final

  return answer

def run_baker_3D(X, R, S):
  """
    This is the main function to call to get an interpolation to X from the input meshes

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

    R = Simplex (4 points, contains X)
    S = extra points (surrounding, in some manner, R and X, but not in R)
  """

  # calculate values only for the triangle
  phi, qlin = qlinear_3D(X, R)

  if len(S.points) == 0:
    answer = {
               'a': None,
               'b': None,
               'c': None,
               'd': None,
               'e': None,
               'f': None,
               'qlin': qlin,
               'error': None,
               'final': None,
              }
    return answer

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

  for (s, q) in zip(S.points, S.q):
    cur_phi, cur_qlin = qlinear_3D(s, R)
    (phi1, phi2, phi3, phi4) = cur_phi

    B.append(
              [
                phi1 * phi2,
                phi1 * phi3,
                phi1 * phi4,
                phi2 * phi3,
                phi2 * phi4,
                phi3 * phi4,
              ]
            )

    w.append(q - cur_qlin)

  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, d, e, f) = np.linalg.solve(A,b)
  except np.linalg.LinAlgError as e:
    smblog.error("linear calculation went bad, resorting to np.linalg.pinv: %s", e)
    (a, b, c, d, e, f) = np.dot(np.linalg.pinv(A), b)

  error_term =  a * phi[0] * phi[1]\
              + b * phi[0] * phi[2]\
              + c * phi[0] * phi[3]\
              + d * phi[1] * phi[2]\
              + e * phi[1] * phi[3]\
              + f * phi[2] * phi[3]

  q_final = qlin + error_term

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

  return answer