# Module  to interpolate functions from irregular samples.
# Last edited on 2021-03-06 11:40:35 by jstolfi

import egg_interp

import rn
import sys
import math

def interpolate(x, p0, p1, p2, p3, cubic):
  # Interpolates a function argument {x} given the 4 surrounding points
  # (2 before {x}, 2 after) as pairs {(xi,yi)}. If {cubic} is true,
  # tries to use a complicated formula involving all four poinys; else
  # uses affine interpolation between {p1} and {p2}, ignoring {p0} and
  # {p3}.
  x0,y0 = p0
  x1,y1 = p1
  x2,y2 = p2
  x3,y3 = p3
  # sys.stderr.write("xi = %.9f %.9f %.9f %.9f\n" % (x0,x1,x2,x3)) 
  assert x0 < x1
  assert x1 <= x and x <= x2
  assert x2 < x3
  if abs(x - x1) < 1.0e-8:
    y = y1
  elif abs(x - x2) < 1.0e-8:
    y = y2
  else:
    assert x2 - x1 >= 1.0e-6
    if not cubic or min(x1-x0, x3-x2) < 0.1*(x2-x1):
      # Affine interpolate:
      r = (x - x1)/(x2 - x1)
      y = (1-r)*y1 + r*y2
    else:
      # Cubic Lagrange interpolation:
      c0 = (x - x1)*(x - x2)*(x - x3)/((x0 - x1)*(x0 - x2)*(x0 - x3))
      c1 = (x - x0)*(x - x2)*(x - x3)/((x1 - x0)*(x1 - x2)*(x1 - x3))
      c2 = (x - x0)*(x - x1)*(x - x3)/((x2 - x0)*(x2 - x1)*(x2 - x3))
      c3 = (x - x0)*(x - x1)*(x - x2)/((x3 - x0)*(x3 - x1)*(x3 - x2))
      y = c0*y0 + c1*y1 + c2*y2 + c3*y3
  return y
  # ----------------------------------------------------------------------
      
def test_interpolate():
  # Tests {interpolate}. 

  sys.stderr.write("%s\n" % ("="*70))
  sys.stderr.write("testing {egg_interp.interpolate}\n")
  for kt in 1,2,3,10:
    for A in 2.0, 0.12345:
      for cubic in False, True:
        test_interpolate_aux(cubic,kt,A)
  sys.stderr.write("%s\n" % ("-"*70))
  return
  # ----------------------------------------------------------------------
       
def test_interpolate_aux(cubic,kt,A):
  # Tests the interpolate function with
  # a function of index {kt} and parameter {A}.
  
  sys.stderr.write("cubic = %s kt = %d A = %.7f\n" % (cubic,kt,A))

  def func(x):
    if kt == 1:
      # Affine
      y = A*x - 1
    elif kt == 2:
      # Quadratic
      y = (A*x - 1)*x + 2
    elif kt == 3:
      # Cubic:
      y = ((A*x - 1)*x + 2)*x - 2
    elif kt == 10:
      # Sinusoid:
      y = math.sin(A/5*x)
    return y
    
  err_max = 0
  for k in range(100):
    x1 = -1 + 0.25*math.cos(17*k)
    x2 = +1 + 0.25*math.cos(17*(k+0.1))
    r = 0.5*(1 + math.cos(31*k))
    x = (1-r)*x1 + r*x2
    x0 = x1 - 1 + 0.50*math.cos(17*(k+0.3))
    x3 = x2 + 1 + 0.50*math.cos(17*(k+0.6))
    y0 = func(x0)
    y1 = func(x1)
    y_cor = func(x)
    y2 = func(x2)
    y3 = func(x3)
    y_int = interpolate(x, (x0,y0), (x1,y1), (x2,y2), (x3,y3), cubic)
    err = y_int - y_cor
    # sys.stderr.write("y_int = %.9f  y_cor = %.9f  err = %.9f\n" % (y_int,y_cor,err))
    if abs(err) > abs(err_max): err_max = err
  sys.stderr.write("max error = %.15f\n" % err_max)
  return
  # ----------------------------------------------------------------------