# Implementation of module {sploine}.
# Last edited on 2021-06-08 14:14:49 by jstolfi

import sploine
import sploine_arc
import rn
import rmxn
import sys
from math import sqrt, hypot, sin, cos, atan2, log, exp, floor, ceil, inf, nan, pi

def interpolate(r, sta, stb):
  # Get the Bézier controls:
  if r == 0:
    str = sta
  elif r == 1:
    str = stb
  else:
    ua, ta, fa = sta
    ub, tb, fb = stb
    txC,tyC,RC,angC, umB,unB = sploine_arc.split_circle_bezier(ua,ta, ub,tb)
    urC, vrC = sploine_arc.eval_circle(r, ua,ub,txC,RC,angC)
    urB, vrB = sploine_arc.eval_bezier_corr(r, umB,unB)
    ur = rn.add(urC, urB)
    vr = rn.add(vrC, vrB)
    tr,zr = rn.dir(vr)
    fr = 0 # Fixation flag
    str = (ur, tr, fr)
  return str
  # ----------------------------------------------------------------------

def interpolate_many(sta, stb, ds):
  ua, ta, fa = sta
  ub, tb, fb = stb
  dab = rn.dist(ua,ub)
  assert dab > 1.0e-6, "points are too close"

  # Get the circle and Bézier params:
  txC,tyC,RC,angC, umB,unB = sploine_arc.split_circle_bezier(ua,ta, ub,tb)

  # Take the length of the circle component as approximation of 
  # the arc length for now:
  if angC == 0:
    assert RC == +inf
    Lab = dab
  else:
    Lab = RC*angC
    
  # Choose the total number of samples {nr} (at least one):
  nr = int(ceil(Lab/ds))
  assert nr >= 1

  # Compute the interpolated states:
  STS = []
  for kr in range(nr):
    if kr == 0:
      str = sta
    else:
      r = kr/nr
      # Add the circle arc and the Bézier correction:
      urC, vrC = sploine_arc.eval_circle(r, ua,ub,txC,RC,angC)
      urB, vrB = sploine_arc.eval_bezier_corr(r, umB,unB)
      ur = rn.add(urC, urB)
      vr = rn.add(vrC, vrB)
      tr,zr = rn.dir(vr)
      # Pack as the state:
      fr = 0 # Fixation flag
      str = (ur, tr, fr)
    STS.append(str)
  
  return STS
  # ----------------------------------------------------------------------
 
def acceleration(sta,stb):
  ua,ta,fa = sta
  ub,tb,fb = stb
  return sploine_arc.acceleration(ua,ta, ub,tb)
  # ----------------------------------------------------------------------

def arc_length(sta, stb):
  ua,ta,fa = sta
  ub,tb,fb = stb
  return sploine_arc.length(ua,ta, ub,tb)
  # ----------------------------------------------------------------------

def circle_example_states(ang, R):
  ua,ta, ub,tb = sploine_arc.circle_example_parms(ang, R)
  
  ua = (10,20,30,40,)
  fa = 2
  sta = (ua, ta, fa) 

  ub = rn.add(ua,wab)
  fb = 1
  stb = (ub, tb, fb) 
  
  return sta,stb
  # ----------------------------------------------------------------------
  
def check_circle_like(sta,stb):
  ua,ta,fa = sta
  ub,tb,fb = stb
  sploine_arc.check_circle_like(ua,ta, ub,tb)
  return
  # ----------------------------------------------------------------------

def example_curves(n):
  assert n >= 3
  STS = []
  
  zer = (0,)*n
  
  def move_to(p,fp):
    STS.append(None)
    STS.append((p,zer,fp))
    return
    # ......................................................................
  
  def arc_to(p,fp):
    assert len(STS) > 0
    st = STS[-1]
    assert st != None
    q,tq,fq = st
    tqp,dqp = rn.dir(rn.sub(p,q))
    if rn.norm(tq) == 0:
      # No velocity, replace:
      tq = tqp
      STS[-1] = (q,tq,fq)
    elif fq == 0:
      # Average velocity with displacement:
      tqnew,mq = rn.dir(rn.add(tq,tqp))
      STS[-1] = (q,tqnew,fq)
    tp,mp = rn.dir(rn.sub(tqp,tq))
    STS.append((p,tp,fp))
    return
    # ......................................................................
  
  def turn_to(v,fnew):
    assert len(STS) > 0
    st = STS[-1]
    assert st != None
    q,t,f = st
    tnew,znew = rn.dir(v)
    STS[-1] = (q,tnew,max(f,fnew))
    return
    # ......................................................................
  
  nc = 6   # Number of sections.
  dX0 = 50 # Horiz displacement between sections.
  X0 = - dX0*(nc-1)/2
  p0 = (X0,100,5,) + (0,)*(n-3)
  t0 = (1,0,0,) + (0,)*(n-3)
  for kc in range(nc):
    p = p0
    t = t0
    move_to(p,(kc % 3))
    turn_to(t,0)
    ns = kc+1 if kc <= 3 else 7 # Num of samples in section.
    for ks in range(ns):
      # Random step length in {[5 _ 15])
      d = 10 + 5*sin(3*kc + 17*ks + 43)
      p = list(rn.mix3(0.95,p, 0.05,p0, d,t))
      p[2] = min(7.5,max(2.5, 0.75*p[2] + 0.25*5))
      p = tuple(p)
      a = [0,]*n
      for i in range(n):
        a[i] = sin((i+1)*kc + (3*i+2)*ks + (7*i+3))
      a = tuple(a)
      a,ma = rn.dir(a)
      v = list(t)
      for i in range(n):
        mi = 1 if i <= 1 else 5 if i == 2 else 20
        v[i] = mi*v[i]
      v = list(rn.mix(0.40,v, 0.60,a))
      for i in range(n):
        mi = 1 if i <= 1 else 5 if i == 2 else 20
        v[i] = v[i]/mi
      t,m = rn.dir(v)
      fp = 0 if ks != nc//2 else (kc+1 % 3)
      arc_to(p,fp)
    turn_to(t,(kc+2 % 3))
    p0 = list(p0)
    p0[0] = p0[0] + dX0
    t0 = [0,]*n
    t0[kc % 2] = 1
    p0 = tuple(p0)
    t0 = tuple(t0)
    
  return STS
  # ----------------------------------------------------------------------