# Module to find the phase of a periodic signal.
# Last edited on 2021-03-06 11:41:12 by jstolfi

import rn
import sys
import math

def find_phase(curve):
  # Given a curve as a list of pairs {(a,v)}, Fits a single-period cosinusoid 
  # of the argument {a} to the values {v} {curve} and returns its phase
  # as an angle in {[0 _ 2*pi)}.  The phase is the argument {a} where that
  # sinusoid is zero.  Assumes that {a} is an angle spanning {[0_2*pi]}
  # and the last point is a closer.
  
  np = len(curve)
  
  per = curve[np-1][0] - curve[0][0] # Period of argument.
  assert abs(per - 2*math.pi) < 1.0e-8
  
  # Find the range of values:
  v_min = +math.inf
  v_max = -math.inf
  for a, v in curve: 
    v_min = min(v_min, v); v_max = max(v_max, v)
  if v_max - v_min < 1.0e-8:
    # Curve is nearly a circle.
    sys.stderr.write("curve is nearly a circle\n")
    phase = 0
  else:
    # Fits a cosine function of increasing
    # frequency in case the curve is rotationally symmetric:
    max_freq = 12
    for f in range(max_freq):
      freq = f + 1
      phase = find_phase_at_freq(curve, freq, v_min, v_max)
      if phase != None: break
    if phase == None:
      # No good period found
      sys.stderr.write("could not detect a phase at freqs 1..%d\n" % max_freq)
  if phase == None: phase = 0 # Last resort.
  return phase
  # ----------------------------------------------------------------------

def find_phase_at_freq(curve, freq, v_min, v_max):
  # Fits a cosine function of frequency {freq}
  # to the values (fudged), and returns the phase
  # of the first max.  Returns {noe} if failed
  # to get a signal.
  
  ts = 0
  tc = 0
  tm = 0
  np = len(curve)
  for k in range(np-1):
    a, v = curve[k]
    y = ((v - v_min)/(v_max - v_min))**4  # To give more weight to the peaks.
    ts += y*math.sin(freq*a)
    tc += y*math.cos(freq*a)
    tm += y*y

  # Did we get a good signal?
  tm = math.sqrt(tm)
  sys.stderr.write("ts = %.9f tc = %.9f sqrt(tm) = %.9f\n" % (ts,tc,tm))
  if math.hypot(ts,tc)/tm > 0.05:
    # We found a freq that fits.
    sys.stderr.write("detected phase at frequency %d\n" % freq)
    phase = math.atan2(-tc,ts) # Shift of frequency {freq} sinusoid in radians.
    while phase < 0: phase += 2*math.pi
    while phase >= 2*math.pi: phase += 2*math.pi
    phase = phase/freq
  else:
    phase = None
  return phase
  # ----------------------------------------------------------------------
  
def test_find_phase():
  # Tests the {find_phase} function.
  sys.stderr.write("%s\n" % ("="*70))
  sys.stderr.write("testing {egg_phase.find_phase}\n")
  for f in range(12):
    freq = f + 1
    test_find_phase_aux(freq)
  sys.stderr.write("%s\n" % ("="*70))
  return
  # ----------------------------------------------------------------------
    
def test_find_phase_aux(freq):
  np = 1237
  phase = 0.1
  sys.stderr.write("--- freq = %.2f --------\n" % freq)
  # 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))

  # Find the phase:
  phase1 = find_phase(curve)
  sys.stderr.write("phase = %.9f phase1 = %.9f\n" % (phase,phase1))
  assert abs(phase - phase1) < 1.0e-3
  return
  # ----------------------------------------------------------------------