#! /usr/bin/python3
# Last edited on 2021-03-28 17:07:16 by jstolfi

# Smooths out the egg contour and computes the curvature (quadratic coef)

import egg_circle
import egg_smooth
import rn
import rmxn
import sys
import math
import re

def main():
  imname = sys.argv[1]
  rsmooth = int(sys.argv[2])
  oprefix = "out/%s-sm%04d" % (imname,rsmooth)
  infile = "out/%s-pts.txt" % imname
  raw = read_contour(infile)
  smt = smooth_outline(raw, rsmooth)
  data = compute_incircles(smt)
  np = len(data)
  ns = min(np, 1000)  # Max points to write.
  write_data(data, ns, oprefix)
  return 0
  # ----------------------------------------------------------------------

def read_contour(fname):
  # Read coordinates of ponts on outline -- two per line, X and Y,
  # as integer multiples of the pixel length. Ignores blank lines.
  #
  # The ouline is not necessarily closed, meaning that the last point 
  # need not be equal to the first one.  However, the two should be 
  # at most 1 pixel apart.
  #
  # Returns the list of pairs {(x[i],y[i]).
  #
  sys.stderr.write("reading raw outline from %s\n" % fname)
  rd = open(fname, 'r', encoding="ISO-8859-1")
  np = 0
  raw = []
  for line in rd:
    line = line.strip()
    fld = re.split(r'[ ]+', line)
    # sys.stderr.write("fld = %s\n" % str(fld))
    if len(fld) == 1 and fld[0] == '':
      # Blank line
      pass
    elif len(fld) == 3:
      ir = int(fld[0])
      xr = float(fld[1])
      yr = float(fld[2])
      assert ir == np
      raw.append((xr, yr))
      np += 1
    else:
      assert False, "bad line format = [%s]" % line
  sys.stderr.write("read %d points\n" % np)
  if rn.dist(raw[0], raw[np-1]) < 0.5:
    sys.stderr.write("outline is closed\n")
    sys.stderr.write("  raw[0] = ( %.9f %.9f )\n" % (raw[0][0], raw[0][1]))
    sys.stderr.write("  raw[%d] =  ( %.9f %.9f )\n" % (np-1,raw[np-1][0],raw[np-1][1]))
    del raw[np-1]
    sys.stderr.write("removing the last point\n")
    np = np - 1
  assert np == len(raw)
  return raw
  # ----------------------------------------------------------------------

def smooth_outline(raw, rsmooth):
  # Smooths the outline with quadratic LSQ fitting using a sliding
  # weight window of radius {rsmooth}.
  # 
  # Returns a list of triples {(p, o, u, cvt)} where {p} is a point of the
  # raw outline, {o} is the corresponding point of the smoothed outline,
  # {u} is the unit tangent vector of the smoothed outline at {o}, and
  # {cvt} is the local curvature the outline.
  #
  # The smoothed contour is not closed, but the last point should be
  # within 1 pixel of the first one.
  sys.stderr.write("smoothing outline with rsmooth = %d\n" % rsmooth)
  np = len(raw)
  smt = []    # Smothed and subsampled outline.
  err_u_max = 0  # Max displacement in the tangent direction.
  err_v_max = 0  # Max displacement in the normal direction.
  p_err_max = None # Point that had max {v} displacement.
  o_err_max = None # Smoothed version of {p_err_max}
  R_prev = None # Radius of curvature at previous data point.
  for j in range(np):
    p = raw[j]
    q = grab_window(raw, j, rsmooth)
    R_ini = R_prev if R_prev != None else rsmooth
    o, u, cvt = egg_smooth.fit_circle(p, q, R_ini)
    vop = rn.sub(o, p)
    err_u = rn.dot(vop, u)
    err_v = rn.dot(vop, (-u[1],+u[0]))
    if abs(err_u) > abs(err_u_max): 
      err_u_max = err_u
    if abs(err_v) > abs(err_v_max): 
      err_v_max = err_v
      p_err_max = p
      o_err_max = o
    smt.append((p,o,u,cvt))
    o_prev = o
    R_prev = 1/cvt
  sys.stderr.write("max point displacement = ( %+.9f %+.9f ):" % (err_u_max,err_v_max))
  sys.stderr.write(" ( %+.9f %+.9f )" % (p_err_max[0],p_err_max[1]))
  sys.stderr.write(" --> ( %+.9f %+.9f )\n" % (o_err_max[0],o_err_max[1]))
  p0, o0, u0, c0 = smt[0]
  pn, on, un, cn = smt[np-1]
  assert rn.dist(p0, pn) < 1.5
  assert rn.dist(o0, on) < 1.5
  return smt
  # ----------------------------------------------------------------------
    
def compute_incircles(smt):
  # Receives a list of triples {(p, o, u, cvt)} as produced by 
  # {smooth_outline}, and adds an additional field {m} that is the 
  # center of the inscribed incircle that goes through {o} with tangent {u}.
  # See {egg_circle.incircles}.
  #
  
  debug = True
  np = len(smt)
  
  # Convert data as expected by {egg_circle.incircles}:
  inds = [ (o, u, cvt) for p, o, u, cvt in smt ]
  
  # Compute the incircles:
  incs = egg_circle.incircles(inds)
  assert len(incs) == np
  
  # Repackage the info:
  data = []
  for k in range(np):
    p, o, u, cvt = smt[k]
    R = 1/cvt
    cin, q = incs[k]
    Ro = rn.dist(cin, o)
    if debug:
      sys.stderr.write("  R = %.9f cin-o = %.9f\n" % (R,Ro))
    assert Ro < R*(1 + 1.0e-6)
    if q != None:
      Rq = rn.dist(cin, q)
      assert (Ro - Rq) < R*1.0e-6
    data.append((p, o, u, cvt, cin))
  return data
  # ----------------------------------------------------------------------

def write_data(data, ns, oprefix):
  # Assumes that {data} is a list of quintuples {(p,o,u,cvt,m)}
  # as produced by {compute_incircles}. 
  # 
  # Compute the arc length, slope angle, and curvature
  # of each point.  
  #
  # Writes the data to {fname}, but only for a subset of {ns} points.
  # 
  # Adds at the end a /closer/, an extra point that has the same values as
  # the first one, except that the arclength and slope angle
  # are one period higher.
  
  otfile = "%s-fit.txt" % oprefix
  
  sys.stderr.write("writing %d + 1 smoothed points to %s\n" % (ns,otfile))
  np = len(data)
  assert ns <= np, "not enough points"
  wr = open(fname, 'w')
  p_prev = None # Previous raw outline point.
  o_prev = None # Previous smoothed outline point.
  s = 0;   # Accumulated smoothed length
  ang_ini = None
  ang_prev = None
  j = 0   # Index of next point in pre-sampled list.
  nwr = 0 # Count of points written.
  for jj in range(ns+1):
    js = math.floor(np*jj/ns + 0.1) # Index of next point to write.
    assert 0 <= js and js <= np
    
    # Process smoothed points from {j} up to {js} to compute accurate arc length:
    while j <= js:
      p, o, u, cvt, m = data[j%np]
      if o_prev != None: 
        # Arclength increase is projection of displacement along {u}:
        vo = rn.sub(o, o_prev)
        ds = rn.dot(vo, u)
        if ds < 0: 
          sys.stderr.write("ds negative at data[%d] = %.9f\n" % (j,ds))
          sys.stderr.write("  p_prev = ( %.9f %.9f )" % (p_prev[0], p_prev[1]))
          sys.stderr.write("  p = ( %.9f %.9f )\n" % (p[0], p[1]))
          sys.stderr.write("  o_prev = ( %.9f %.9f )" % (o_prev[0], o_prev[1]))
          sys.stderr.write("  o = ( %.9f %.9f )\n" % (o[0], o[1]))
        s += ds
      o_prev = o
      p_prev = p
      j = j + 1
    
    # Compute the slope angle {ang} at the subsampled point, ensure monotonic:
    ang = math.atan2(u[1],u[0])
    if ang_prev != None:
      # Make sure that {ang} is "continuous" in spite of wrap-around:
      # sys.stderr.write("ang_prev = %.9f ang = %.9f" % (ang_prev,ang))
      while ang > ang_prev + math.pi/6: ang -= 2*math.pi
      while ang < ang_prev - math.pi/6: ang += 2*math.pi
      # sys.stderr.write(" --> %.9f\n" % ang)
      # But beware of spurious wrap-around:
      if ang < ang_prev:
        sys.stderr.write("!! concavity detected at sample %d (orig %d):\n" % (jj,j-1))
        sys.stderr.write("   ang_prev = %.14f ang = %.14f" % (ang_prev,ang))
        sys.stderr.write(" change = %.14f\n" % (ang - ang_prev))
        # assert False
    # Write the sampled data:
    wr.write("%8.3f  %8.3f %8.3f" % (s,p[0],p[1]))
    wr.write("  %8.3f %8.3f  %8.3f %8.3f" % (o[0],o[1],u[0],u[1]))
    wr.write("  %+10.9f %+10.9f" % (cvt,ang))
    wr.write("  %+10.9f %+10.9f\n" % (m[0],m[1]))
    nwr += 1
    if ang_ini == None: ang_ini = ang
    ang_prev = ang
  assert j == np+1
  assert nwr == ns+1
  sys.stderr.write("contour length L = %.9f\n" % s)
  assert abs((ang_prev - ang_ini) - 2*math.pi) < 1.0e-8
  wr.close()

def grab_window(raw, j, rsmooth):
  # Collects points of the contour around {raw[i]} until a radius of {rsmooth}.
  # Wraps around the list {raw}.
  # Returns a list of points.
  np = len(raw)
  pj = raw[j%np]
  ilo = j
  ihi = j
  nq = 1  # Number of points in window.
  while nq < np//2:
    if rn.dist(raw[(ilo-1)%np], pj) <= rsmooth:
      ilo = ilo - 1; nq += 1
    else:
      break
    if rn.dist(raw[(ihi+1)%np], pj) <= rsmooth:
      ihi = ihi + 1; nq += 1
    else:
      break
  assert nq <= np//2 + 1
  assert nq == ((ihi - ilo + 1) % np)
  
  q = [ raw[(ilo + k) % np] for k in range(nq) ]

  wrad = min(rn.dist(q[0], pj), rn.dist(q[nq-1], pj))
  if wrad < 0.80*rsmooth:
    if j < 3: sys.stderr.write("window narrower than requested wrad = %.9f\n" % wrad)
  return q
  # ----------------------------------------------------------------------
    
main()