"""Exact Lipschitz endpoint bounds for continuous collision clearance.

Run in the intended media directory. Uses the repository NumPy/Matplotlib
requirements and writes one opaque PNG to the working directory. All distances
are constructed model values, not measurements.
"""
from pathlib import Path
import matplotlib
matplotlib.use('Agg')
import matplotlib.pyplot as plt
import numpy as np


def endpoint_bounds(time_s):
    left = 12.0 - 100.0 * time_s
    right = 20.0 - 100.0 * (0.1 - time_s)
    return left, right, np.maximum(left, right)


def main():
    time_s = np.linspace(0.0, 0.1, 1001)
    left, right, lower = endpoint_bounds(time_s)
    fig, ax = plt.subplots(figsize=(7.4, 4.5), dpi=100, facecolor='white')
    ax.set_facecolor('white')
    ax.plot(1000*time_s, left, color='#b36a32', lw=1.7, ls='--',
            label=r'Left endpoint: $d_0-Lt$')
    ax.plot(1000*time_s, right, color='#9160a5', lw=1.7, ls='--',
            label=r'Right endpoint: $d_1-L(h-t)$')
    ax.plot(1000*time_s, lower, color='#155c95', lw=3,
            label='Combined lower bound')
    ax.axhline(10, color='#31804a', lw=1.5, ls=':',
               label='Required margin: 10 mm')
    ax.axhline(7, color='#666666', lw=1.2, ls='-.',
               label='Simpler constant bound: 7 mm')
    ax.scatter([0, 100], [12, 20], color='#155c95', s=35, zorder=5)
    ax.scatter([10], [11], color='#155c95', s=30, zorder=5)
    ax.annotate('Guaranteed minimum\n11 mm', xy=(10, 11),
                xytext=(17, 16), fontsize=9,
                arrowprops={'arrowstyle':'->', 'color':'#155c95'}, color='#155c95')
    ax.text(0.5, 0.95, 'Endpoint clearances: 12 and 20 mm\nL = 100 mm/s; interval = 100 ms',
            transform=ax.transAxes, ha='center', va='top', fontsize=9)
    ax.set(xlim=(-2, 102), ylim=(0, 22), xlabel='Time from first sample (ms)',
           ylabel='Clearance / lower bound (mm)')
    ax.set_title('Two endpoint samples certify intermediate clearance', fontsize=12)
    ax.legend(loc='lower left', fontsize=8, framealpha=1)
    ax.grid(alpha=.14)
    fig.text(.5, .016, 'Original geometric certificate; the blue curve is a bound, not a measured path.',
             ha='center', fontsize=8.5, color='#555555')
    fig.tight_layout(rect=(0, .035, 1, 1))
    fig.savefig(Path('endpoint-clearance-cones.png'), dpi=100,
                facecolor='white', transparent=False)
    plt.close(fig)


if __name__ == '__main__':
    main()