"""Exact point-path clearance and adaptive Lipschitz certificates; PNG in cwd."""
from pathlib import Path
import numpy as np
import matplotlib.pyplot as plt


def certify(delta=.01,margin=.005):
    sampled={}
    intervals=[]
    def query(t):
        if t not in sampled:sampled[t]=float(np.hypot(t-.5,delta))
        return sampled[t]
    def visit(a,b,depth=0):
        assert depth<=24
        lower=min(query(a),query(b))-(b-a)/2
        if lower>margin:
            intervals.append((a,b,lower));return
        midpoint=(a+b)/2
        assert query(midpoint)>margin
        visit(a,midpoint,depth+1);visit(midpoint,b,depth+1)
    visit(0.,1.)
    return sampled,intervals


def main():
    sampled,intervals=certify()
    assert min(np.hypot(-.5,.01),np.hypot(.5,.01))-.5 < .005
    assert len(sampled)>2
    assert np.isclose(sum(b-a for a,b,_ in intervals),1)
    for a,b,lower in intervals:
        assert lower>.005
        exact_min=np.hypot(np.clip(.5,a,b)-.5,.01)
        assert lower<=exact_min+1e-14
    widths=[(b-a,(a+b)/2)for a,b,_ in intervals]
    assert min(h for h,t in widths if abs(t-.5)<.1)<max(h for h,t in widths if abs(t-.5)>.3)
    t=np.linspace(0,1,2001)
    times=np.array(sorted(sampled))
    fig,axes=plt.subplots(1,2,figsize=(9.6,4),dpi=100,facecolor='white')
    axes[0].plot(t,np.hypot(t-.5,.01),color='#333333',lw=1.7,label='Exact constructed clearance')
    axes[0].scatter(times,[sampled[x]for x in times],color='#2475ad',s=25,label=f'{len(sampled)} queried times',zorder=3)
    axes[0].axhline(.005,color='#b63f32',ls='--',lw=1,label='Required clearance: 0.005 m')
    for i,(a,b,lower)in enumerate(intervals):
        axes[0].plot([a,b],[lower,lower],color='#38824d',lw=2,label='Certified interval lower bound'if i==0 else None)
    axes[0].set(xlabel='Time (s)',ylabel='Clearance (m)',title='Conservative bounds certify the whole path',ylim=(0,.55))
    axes[0].legend(fontsize=8,loc='upper center')
    for a,b,_ in intervals:axes[1].plot([a,b],[b-a,b-a],color='#2475ad',lw=3)
    axes[1].set_yscale('log',base=2)
    axes[1].set(xlabel='Time (s)',ylabel='Certified interval duration (s)',title='Shorter intervals near closest approach')
    for ax in axes:ax.grid(alpha=.2)
    fig.tight_layout()
    fig.savefig(Path.cwd()/(Path(__file__).stem+'.png'),facecolor='white')
    plt.close(fig)
    print(f'{len(sampled)} sample queries; {len(intervals)} certified intervals; minimum true clearance = 0.01 m')


if __name__=='__main__':
    main()