# Module to resample closed curves.
# Last edited on 2021-03-05 08:27:56 by jstolfi

import egg_interp
import rn
import sys
import math

def resample(curve, ns, phase):
  # Assumes that the points of curve are {(a,v)}, that argument {a} has
  # period {2*pi}, and that the last point is a closer.   Resamples
  # the given {curve} by interpolation to {ns} equally spaced argument
  # values spannning {[0_2*pi]}, shifting it by {-phase}.
  #
  # The first point of the result will have argument 0. The last point will be a closer.
  
  np = len(curve)
  np1 = np - 1  # Input points minus the closer.
  
  def get_pt(i):
    # Returns point {i%np} of the curve, with argument
    # shifted by the proper multiple of {2*pi}.
    ai, vi = curve[i%np1]
    return (ai + (i//np1)*2*math.pi, vi)

  i1 = 1
  res = []
  # Computes all {ns-1} new samples except the closer:
  ns1 = ns - 1 
  for k in range(ns1):
    ak = 2*math.pi*k/(ns1)
    
    # Find two consecutive indices {i1,i2} such that {ak+phase},
    # possibly incremented by a multiple of {2*pi}, is between the
    # arguments of {curve[i1]} and {curve[i2]}. All indices are taken
    # modulo {np-1}, with multiples of {2*pi} added to the arguments
    # as appropriate.
    bk = ak + phase
    while bk < get_pt(i1)[0]: bk += 2*math.pi
    i2 = i1+1
    while bk > get_pt(i2)[0]: i1 = i2; i2 = i2+1
    
    # Now interpolate the value at {bk} from the 4 surrounding points:
    i0 = i1-1
    i3 = i2+1
    # sys.stderr.write("ii = %d %d %d %d\n" % (i0,i1,i2,i3))
    cubic = True
    vk = egg_interp.interpolate(bk, get_pt(i0), get_pt(i1), get_pt(i2), get_pt(i3), cubic)
    
    # Add sampled point to curve:
    res.append((ak, vk))
    
  # Add closer point:
  assert res[0][0] == 0
  res.append((2*math.pi, res[0][1]))
  assert len(res) == ns
  return res
  # ----------------------------------------------------------------------
  
def test_resample():
  # Tests the {resample} function.

  sys.stderr.write("%s\n" % ("="*70))
  sys.stderr.write("testing {egg_resample.resample}\n")
  np = 1237
  phase = 0.1
  freq = 3

  # Make a sinusoid of frequency {freq} and given {phase}
  curve = []
  for k in range(np):
    ak = 2*math.pi*k/(np-1)
    vk = math.sin(freq*(ak - phase))
    curve.append((ak,vk))

  # Resample with alignment:
  ns = 1001
  res = resample(curve, ns, phase)
  assert len(res) == ns

  # Check result:
  e2 = 0
  for a, v in res:
    v1 = math.sin(freq*a)
    e2 += (v - v1)**2
  err = math.sqrt(e2/ns)
  sys.stderr.write("err = %.9f\n" % err)
  assert err < 1.0e-5*freq*freq

  sys.stderr.write("%s\n" % ("="*70))
  return
  # ----------------------------------------------------------------------