#!/usr/bin/env python3
"""A paired, analytic grasp test that isolates missing contact information."""
import argparse
import csv
import json
import math
import random
from collections import Counter
from pathlib import Path

G = 9.81
TRAIN_PAIRS = 480
TEST_PAIRS = 360
STUDIES = {
    'overlap': (5361, .018),
    'separated': (9173, .018),
    'noisier_contact': (19937, .055),
}
CONDITIONS = ('visual_prior', 'calibrated_joint_feedback', 'joint_and_contact_feedback')


def attempted_grasps(seed, n, family, noise):
    rng = random.Random(seed)
    rows = []
    for pair_id in range(n):
        # Supply the same known mass and requested acceleration to both members,
        # even though the load would generally need its own measurement in RGB.
        mass = rng.uniform(.19, .225)
        acceleration = rng.uniform(2.3, 3.4)
        load = mass * (G + acceleration)
        gain = rng.uniform(.76, 1.24)
        measured_gain = gain + rng.gauss(0, .018)
        for surface in ('fragile_grippy', 'slick_rigid'):
            fragile = surface == 'fragile_grippy'
            if family == 'overlap':
                mu = rng.uniform(.53, .72) if fragile else rng.uniform(.36, .54)
                safe_max = rng.uniform(3.4, 4.7) if fragile else rng.uniform(5.3, 7)
            else:
                mu = rng.uniform(.62, .83) if fragile else rng.uniform(.25, .33)
                safe_max = rng.uniform(2.40, 3.20) if fragile else rng.uniform(6.2, 8.4)
            # While the fixture still supports the object, a tangential ramp
            # at a known low normal load yields a noisy, quantized slip signal.
            probe_step = .03
            first_probe = math.ceil(max(.06, mu + rng.gauss(0, noise)) / probe_step)
            probe_center = (first_probe - .5) * probe_step
            rows.append(dict(pair=pair_id, surface=surface, mass_kg=mass, upward_accel_m_s2=acceleration,
                             load_n=load, friction=mu, stable_min_n=load/(2*mu), damage_above_n=safe_max,
                             actuator_gain=gain, measured_gain=measured_gain, probe_bin=first_probe,
                             probe_center=probe_center))
    return rows


def predict_friction(coef, observed):
    intercept, slope = coef
    return max(.12, min(1.0, intercept+slope*observed))


def classify(attempt, command):
    actual = command * attempt['actuator_gain']
    if actual > attempt['damage_above_n']:
        return 'damage', actual
    if actual < attempt['stable_min_n']:
        return 'slip', actual
    return 'success', actual


def control(attempt, condition, fitted):
    load = attempt['load_n']
    if condition == 'visual_prior':
        return load * fitted['visual_scale']
    if condition == 'calibrated_joint_feedback':
        return (load * fitted['joint_scale']) / attempt['measured_gain']
    mu = predict_friction(fitted['friction_fit'], attempt['probe_center'])
    return (load * fitted['probe_margin'] / (2*mu)) / attempt['measured_gain']


def learn(demos):
    x_bar = sum(d['probe_center'] for d in demos) / len(demos)
    mu_bar = sum(d['friction'] for d in demos) / len(demos)
    numerator = sum((d['probe_center']-x_bar)*(d['friction']-mu_bar) for d in demos)
    denominator = sum((d['probe_center']-x_bar)**2 for d in demos)
    slope = numerator / denominator
    fitted = {'friction_fit':(mu_bar-slope*x_bar, slope)}
    for condition, knob, grid in (
        ('visual_prior','visual_scale',(i*.01 for i in range(45,280))),
        ('calibrated_joint_feedback','joint_scale',(i*.01 for i in range(45,280))),
        ('joint_and_contact_feedback','probe_margin',(i*.005 for i in range(200,251))),
    ):
        best = -1
        for setting in grid:
            fitted[knob] = setting
            score = sum(classify(d, control(d, condition, fitted))[0]=='success' for d in demos)
            if score > best:
                best, winner = score, setting
        fitted[knob] = winner
    return fitted


def run():
    # Use balanced contact regimes in training. The held-out test seeds and
    # added probe noise are fixed independently of the calibration scores.
    examples = [row for i, family in enumerate(('overlap','separated'))
                for row in attempted_grasps(1092+i, TRAIN_PAIRS//2, family, .018)]
    fitted = learn(examples)
    results = {}
    trial_rows = []
    for name, (seed, noise) in STUDIES.items():
        family = 'overlap' if name=='overlap' else 'separated'
        cases = attempted_grasps(seed, TEST_PAIRS, family, noise)
        ambiguous = 0
        for a,b in zip(cases[::2],cases[1::2]):
            assert all(a[key]==b[key] for key in ('load_n','mass_kg','actuator_gain','measured_gain'))
            assert all(d['stable_min_n']<=d['damage_above_n'] for d in (a,b))
            ambiguous += max(a['stable_min_n'],b['stable_min_n']) > min(a['damage_above_n'],b['damage_above_n'])
        metrics = {}
        for policy in CONDITIONS:
            outcomes = Counter()
            for row in cases:
                command = control(row, policy, fitted)
                outcome, normal = classify(row, command)
                outcomes[outcome] += 1
                trial_rows.append(dict(domain=name, condition=policy, **row, requested_n=command,
                                       achieved_n=normal, outcome=outcome))
            metrics[policy] = {outcome:outcomes[outcome] for outcome in ('success','slip','damage')}
        results[name] = dict(pairs=TEST_PAIRS, trials_per_policy=len(cases), disjoint_pairs=ambiguous,
                             shared_exact_force_upper_bound=len(cases)-ambiguous,
                             policies=metrics, probe_noise_std=noise)
    example = attempted_grasps(STUDIES['separated'][0], 1, 'separated', STUDIES['separated'][1])
    example_windows = {
        case['surface']:[round(case['stable_min_n'],2),round(case['damage_above_n'],2)]
        for case in example
    }
    return dict(status='observed_current', created_at='2026-11-19',
                scope='Analytic simulated contact decision. No physical sensor readings or video-policy training.',
                training_pairs=TRAIN_PAIRS, calibration=fitted, example_windows_n=example_windows,
                test=results), trial_rows


def main():
    parser = argparse.ArgumentParser(description=__doc__)
    parser.add_argument('--output-dir', type=Path, default=Path(__file__).resolve().parent)
    args = parser.parse_args()
    args.output_dir.mkdir(parents=True, exist_ok=True)
    result, trials = run()
    (args.output_dir/'contact-results.json').write_text(json.dumps(result, indent=2) + '\n')
    with (args.output_dir/'contact-trials.csv').open('w', newline='') as handle:
        writer = csv.DictWriter(handle, fieldnames=trials[0].keys())
        writer.writeheader()
        writer.writerows(trials)
    print(json.dumps(result['test'],indent=2))


if __name__ == '__main__':
    main()
