"""CREATIO CT-02. Run: python3 spatial-validation.py (standard library only).
Coordinates use model length unit u, time s, angles rad. Independent finite
differences check the original Cartesian mapping against analytic derivatives.
"""
import math as m, json, csv
from pathlib import Path
ROOT=Path(__file__).resolve().parent
R0,D,A,Kr,Ka=.68,1.72,.18,1,6
k=2*m.pi*Kr/D
def add(a,b): return tuple(x+y for x,y in zip(a,b))
def mul(s,a): return tuple(s*x for x in a)
def norm(a): return m.sqrt(sum(x*x for x in a))
def dot(a,b): return sum(x*y for x,y in zip(a,b))
def cross(a,b): return (a[1]*b[2]-a[2]*b[1],a[2]*b[0]-a[0]*b[2],a[0]*b[1]-a[1]*b[0])
def laws(t,g,moving):
 return (.23+.31*t,.5+.47*t,g+(.04*m.sin(.7*t) if moving else 0))
def point(r,a,t,g,moving=False):
 pr,pa,G=laws(t,g,moving)
 w=A*m.sin(k*r+pr)*m.cos(Ka*a+pa)
 rho=R0+r*m.cos(G)-w*m.sin(G)
 return (rho*m.cos(a),rho*m.sin(a),r*m.sin(G)+w*m.cos(G))
def analytic(r,a,t,g,moving=False):
 pr,pa,G=laws(t,g,moving); u=k*r+pr;v=Ka*a+pa
 w=A*m.sin(u)*m.cos(v);wr=A*k*m.cos(u)*m.cos(v);wa=-A*Ka*m.sin(u)*m.sin(v)
 wt=A*(.31*m.cos(u)*m.cos(v)-.47*m.sin(u)*m.sin(v))
 gd=.028*m.cos(.7*t) if moving else 0
 er=(m.cos(a),m.sin(a),0);ea=(-m.sin(a),m.cos(a),0)
 eg=add(mul(m.cos(G),er),(0,0,m.sin(G)));en=add(mul(-m.sin(G),er),(0,0,m.cos(G)))
 rho=R0+r*m.cos(G)-w*m.sin(G)
 cr=add(eg,mul(wr,en));ca=add(mul(rho,ea),mul(wa,en))
 ct=add(mul(wt+gd*r,en),mul(-gd*w,eg))
 return cr,ca,ct,rho,wr,wa
samples=[(D*i/8,2*m.pi*j/24,t,m.radians(g),False) for g in range(0,91,5) for i in range(9) for j in range(24) for t in (0,.37,1.11)]
samples += [(D*i/8,2*m.pi*j/24,t,m.pi/4,True) for i in range(9) for j in range(24) for t in (0,.37,1.11)]
rows=[]
for h in (.01,.003,.001,.0003,.0001,.00001):
 errors=[0.,0.,0.]
 for r,a,t,g,moving in samples:
  an=analytic(r,a,t,g,moving)
  for axis in range(3):
   p=[r,a,t];q=p.copy();p[axis]+=h;q[axis]-=h
   fd=mul(1/(2*h),add(point(*p,g,moving),mul(-1,point(*q,g,moving))))
   errors[axis]=max(errors[axis],norm(add(fd,mul(-1,an[axis]))))
 rows.append(dict(h=h,cR=errors[0],cAlpha=errors[1],ct=errors[2]))
identity=orth=unit=seam=0.; minj=1e9
for r,a,t,g,moving in samples:
 cr,ca,ct,rho,wr,wa=analytic(r,a,t,g,moving);v=cross(cr,ca);J=norm(v);n=mul(1/J,v)
 minj=min(minj,J);identity=max(identity,abs(J-m.sqrt(rho*rho*(1+wr*wr)+wa*wa)))
 orth=max(orth,abs(dot(n,cr))/norm(cr),abs(dot(n,ca))/norm(ca));unit=max(unit,abs(norm(n)-1))
 seam=max(seam,norm(add(point(r,a+2*m.pi,t,g,moving),mul(-1,point(r,a,t,g,moving)))))
feature_error=0.; pitch_error=0.; acceleration_error=0.
for gd in range(0,91,5):
 g=m.radians(gd)
 def feature(t):
  r=(m.pi/2-(.23+.31*t))/k;a=(m.pi-(.5+.47*t))/Ka
  return point(r,a,t,g)
 for t in (0,.2,.5):
  r=(m.pi/2-(.23+.31*t))/k;a=(m.pi-(.5+.47*t))/Ka;rho=R0+r*m.cos(g)+A*m.sin(g)
  vr=-.31/k;om=-.47/Ka;dr=vr*m.cos(g)
  v=(dr*m.cos(a)-rho*om*m.sin(a),dr*m.sin(a)+rho*om*m.cos(a),vr*m.sin(g))
  acc=(-rho*om*om*m.cos(a)-2*dr*om*m.sin(a),-rho*om*om*m.sin(a)+2*dr*om*m.cos(a),0)
  h=1e-4
  fd=mul(1/(2*h),add(feature(t+h),mul(-1,feature(t-h))))
  fdd=mul(1/(h*h),add(add(feature(t+h),feature(t-h)),mul(-2,feature(t))))
  feature_error=max(feature_error,norm(add(fd,mul(-1,v))));acceleration_error=max(acceleration_error,norm(add(fdd,mul(-1,acc))))
 pitch_error=max(pitch_error,abs(2*m.pi*(-.31/k)/(-.47/Ka)-D*Ka/Kr*.31/.47))
result=dict(study='CT-02',parameters=dict(R0=R0,deltaR=D,A=A,Kr=Kr,Ka=Ka,units='u, s, rad'),samples_per_step=len(samples),gamma_degrees=list(range(0,91,5)),phase_laws='phi_r=.23+.31t; phi_a=.5+.47t',extra_gamma_law='pi/4+.04 sin(.7t)',convergence=rows,min_sampled_J=minj,analytic_global_lower_bound_J=.5,max_J_identity_error=identity,max_normal_orthogonality_error=orth,max_normal_length_error=unit,max_periodic_seam_error=seam,max_feature_velocity_error=feature_error,max_feature_acceleration_error=acceleration_error,pitch_identity_error=pitch_error,scope='Mathematical implementation verification, not experimental or contact validation. Central differences at R endpoints use the smooth analytic extension.')
assert rows[2]['cAlpha']<rows[0]['cAlpha']/90
assert max(rows[-1][x] for x in ('cR','cAlpha','ct'))<1e-7
assert minj>=.5-1e-12 and orth<1e-12 and identity<1e-12 and seam<1e-12
assert feature_error<1e-7 and acceleration_error<1e-6
(ROOT/'spatial-validation.json').write_text(json.dumps(result,indent=2)+'\n')
with (ROOT/'spatial-convergence.csv').open('w') as f:
 w=csv.DictWriter(f,fieldnames=rows[0].keys());w.writeheader();w.writerows(rows)
print(json.dumps(result,indent=2))