#!/usr/bin/env python3
import sys, numpy as np
sys.path.insert(0, '/home/m/re-tools')
from render_parity import load
from scipy.interpolate import PchipInterpolator
from scipy.optimize import minimize
import wave

BT='/home/m/soothe-bt/'; FS=44100.0; GAIN=4.132

def bandres(f, fc, Q):
    w0 = fc*2*np.pi/FS; c,s=np.cos(w0),np.sin(w0)
    p=(s*0.5)/Q; a,a2=p*GAIN,p/GAIN
    A=[a+1,-2*c,1-a]; B=[a2+1,-2*c,1-a2]
    w=2*np.pi*np.asarray(f)/FS; z=np.exp(-1j*w)
    return np.abs(2.0*(B[0]+B[1]*z+B[2]*z*z)/(A[0]+A[1]*z+A[2]*z*z))

def warp(f): x=np.asarray(f)/2000.0; return 0.87*7.942*x/(7.942+x)

LX = np.array([-0.75,-0.5012,-0.5,-0.2012,0.0988,0.2488,0.3988,0.5488,0.574,0.61,0.75,1.0])
LY = np.array([0.4402,0.366,0.4552,0.459,0.541,0.576,0.608,0.636,0.5645,0.6471,0.6562,0.6670])
_ip=PchipInterpolator(LX,LY); _lymin,_lymax=LY.min(),LY.max()
def lut(x): return np.clip(_ip(np.asarray(x)),_lymin,_lymax)

def frames(x, fc, Q, G_, W_, A_, rp, NS=2048, hop=512, tatt=0.011, trel=0.08):
    win=np.sqrt(np.hanning(NS)); wsum=win.sum(); n=len(x)
    nfr=max(1,int(np.ceil((n-NS)/hop))+1)
    X=np.empty((nfr,NS//2+1),dtype=complex); freqs=np.fft.rfftfreq(NS,1/FS)
    res=bandres(freqs,fc,Q)
    att=np.exp(-hop/(tatt*FS)); rel=np.exp(-hop/(trel*FS))
    am=np.zeros(freqs.size); G=np.empty(X.shape)
    for m in range(nfr):
        s=m*hop; seg=np.zeros(NS); kk=min(NS,n-s); seg[:kk]=x[s:s+kk]
        F=np.fft.rfft(win*seg); X[m]=F; ac=2*np.abs(F)/wsum
        am=np.where(ac>am, att*am+(1-att)*ac, rel*am+(1-rel)*ac)
        xv=np.log10(np.maximum(am/np.maximum(res,1e-12),1e-9))
        C = G_*lut(xv) + W_*warp(freqs)**A_
        gain = np.maximum(1-np.minimum(C,0.95),1e-9)
        gain = gain * np.power(np.maximum(res,1e-12), rp)
        G[m] = gain
    return X,G,win,hop,n

def synthe(X,G,win,hop,n):
    out=np.zeros(n); acc=np.zeros(n); NS=len(win)
    for m in range(X.shape[0]):
        seg=np.fft.irfft(X[m]*G[m])*win; s=m*hop; lay=min(NS,n-s)
        out[s:s+lay]+=seg[:lay]; acc[s:s+lay]+=(win*win)[:lay]
    return out/np.maximum(acc,1e-12)

def tone_cmp(x,f,seglen=0.75*FS):
    x=np.asarray(x)[-int(seglen):]; n=len(x); t=np.arange(n)/FS; w=2*np.pi*f
    return np.hypot(2*np.sum(x*np.cos(w*t))/n,2*np.sum(x*np.sin(w*t))/n)

def dB(v): return 20*np.log10(np.clip(v,1e-9,None))

x=np.mean(load(BT+'dual.wav'),axis=1)
refs=[(q,f'dual_b1q_{q}.wav',f) for q in [0.1,1.0,10.0] for f in (500,2000)]
tone_ref={(r,f):dB(tone_cmp(np.mean(load(BT+r),axis=1),f)) for _,r,f in refs}

# Joint fit: G,W,A,rp with G>0 constraint via log transform
def obj(logp):
    lG,W_,A_,rp = logp; G_=np.exp(lG)
    tot=[]
    for q,ref,f in refs:
        X,G,win,hop,n=frames(x,500.0,q,G_,W_,A_,rp)
        y=synthe(X,G,win,hop,n); tot.append(dB(tone_cmp(y,f))-tone_ref[(ref,f)])
    return np.mean(np.abs(tot))

best=(999,None)
for p0 in [np.log([1.0,0.3,1.0,0.1]), np.log([0.8,0.4,1.5,0.2]), np.log([0.6,0.5,2.0,0.3])]:
    r = minimize(obj, p0, method='Nelder-Mead', options=dict(maxiter=5000, xatol=1e-6, fatol=1e-6))
    if r.fun < best[0]: best=(r.fun, r.x)

p=best[1]; G_=np.exp(p[0])
print(f'BEST: G={G_:.4f} W={p[1]:.4f} A={p[2]:.4f} rp={p[3]:.4f} mean={best[0]:.3f}')
for q,ref,f in refs:
    X,G,win,hop,n=frames(x,500.0,q,G_,p[1],p[2],p[3])
    y=synthe(X,G,win,hop,n)
    print(f'  {ref} tone{f}: err={dB(tone_cmp(y,f))-tone_ref[(ref,f)]:+.2f}')

# al_* validation with this model
print('\nal_*:')
for lv in [3,6,9,12,18,24]:
    xi=np.mean(np.frombuffer(open(f'{BT}lvl_tone_lv{lv}.wav','rb').read(),dtype=np.int16).astype(float).reshape(-1,1)/32768.0,axis=1)
    xo=np.mean(load(f'{BT}al_{lv}.wav'),axis=1)
    X,G,win,hop,n=frames(xi,1000.0,0.9999978,G_,p[1],p[2],p[3])
    y=synthe(X,G,win,hop,n)
    mr=dB(tone_cmp(xo,1000))-dB(tone_cmp(xi,1000))
    mo=dB(tone_cmp(y,1000))-dB(tone_cmp(xi,1000))
    print(f'  lv{lv}: ref={mr:+.2f} out={mo:+.2f} err={mo-mr:+.2f}')