#!/usr/bin/env python3
"""Compare moving-grasp tracking and colour normalization on held-out cases."""
import argparse
import csv
import json
from pathlib import Path

import numpy as np

from simulate import DEADLINE, HZ, SCHEDULES, aggregate, fit, fit_sorter, rollout, scenarios

SEEDS = (61041, 62921, 65809)
TRAIN = 50
HOLDOUT = 120
DOMAINS = ('reference', 'shifted', 'expanded')
CONDITIONS = {
    'native': ('native', False, False, False, 155),
    'uniform_2x': ('uniform_2x', False, False, False, 155),
    'delay_only': ('phase_aware', True, False, False, 155),
    'moving_grasp': ('phase_aware', False, True, False, 155),
    'moving_grasp_normalized': ('phase_aware', False, True, True, 155),
    'full': ('phase_aware', False, True, True, 180),
}


def cases(seed, count, domain):
    data = scenarios(np.random.default_rng(seed), count, 'shifted' if domain == 'expanded' else domain)
    rng = np.random.default_rng(seed + 1)
    data['measured_belt'] = data['belt'] + rng.normal(0, .0025, count)
    if domain == 'expanded':
        data['belt_accel'] = rng.uniform(-.012, .016, count)
        data['camera_bias'] = rng.uniform(-.16, .16, count)
        data['occlusion'] = rng.uniform(.10, .36, count)
    return data


def paired(a, b, key):
    assert [(r['seed'], r['case']) for r in a] == [(r['seed'], r['case']) for r in b]
    delta = np.array([int(x[key]) - int(y[key]) for x, y in zip(a, b)]).reshape(len(SEEDS), HOLDOUT)
    rng = np.random.default_rng(310941)
    # Keep training seeds fixed. Resample matched cases independently inside each seed.
    sampled = np.array([rng.choice(batch, (3000, HOLDOUT), replace=True) for batch in delta])
    lower, upper = np.quantile(sampled.mean(axis=(0, 2)), [.025, .975])
    return [round(float(v), 4) for v in (delta.mean(), lower, upper)]


def fit_policies(seed, training):
    controller = {}
    for schedule, sweep in [(name, 155) for name in SCHEDULES] + [('phase_aware', 180)]:
        demo = rollout(training, SCHEDULES[schedule], np.random.default_rng(seed + 30), roll_degrees=sweep)
        controller[schedule, sweep] = fit(demo['samples'])

    # Both 155-degree vision models see the same 50 trajectories, view angles,
    # pixels, and inspection sampling cadence.
    vision = {}
    for normalize, sweep in [(False, 155), (True, 155), (True, 180)]:
        demo = rollout(training, SCHEDULES['phase_aware'], np.random.default_rng(seed + 20),
                       track_belt=True, normalize_color=normalize, roll_degrees=sweep)
        vision[normalize, sweep] = fit_sorter(demo['stats'], training['defect'])
    return controller, vision


def run():
    grouped = {domain: {name: [] for name in CONDITIONS} for domain in DOMAINS}
    for seed in SEEDS:
        training = cases(seed, TRAIN, 'reference')
        motion, vision = fit_policies(seed, training)
        for index, domain in enumerate(DOMAINS):
            test = cases(seed + 10000 * (index + 1), HOLDOUT, domain)
            for name, (schedule, compensate, track, normalize, sweep) in CONDITIONS.items():
                result = rollout(test, SCHEDULES[schedule],
                                 np.random.default_rng(seed + 300 + index), motion[schedule, sweep],
                                 vision[normalize, sweep], compensate, track_belt=track,
                                 normalize_color=normalize, roll_degrees=sweep)
                for i in range(HOLDOUT):
                    inspected = bool(result['inspected'][i])
                    success = bool(result['success'][i])
                    defect = bool(test['defect'][i])
                    reason = str(result['reasons'][i])
                    elapsed = float(result['seconds'][i])
                    grouped[domain][name].append(dict(
                        domain=domain, condition=name, seed=seed, case=i, defective=defect,
                        inspected=inspected, correct_route=bool(defect == result['route'][i]) if inspected else None,
                        success=success, on_time=success and elapsed <= DEADLINE,
                        seconds=round(elapsed, 6), roll_deg=round(float(result['roll_deg'][i]), 3),
                        placement_error_mm=round(1000*float(result['placement_error'][i]), 3), outcome=reason,
                    ))
    stats = {}
    for domain, group in grouped.items():
        arms = {}
        for name, trials in group.items():
            stats_row = aggregate(trials)
            defective = [r for r in trials if r['defective']]
            stats_row['end_to_end_defects_correct'] = sum(row['success'] for row in defective)
            stats_row['end_to_end_defects_total'] = len(defective)
            arms[name] = stats_row
        stats[domain] = {
            'arms': arms,
            'paired_effects': {
                'full_vs_uniform_on_time': paired(group['full'], group['uniform_2x'], 'on_time'),
                'full_vs_native_total': paired(group['full'], group['native'], 'success'),
                'moving_vs_delay_on_time': paired(group['moving_grasp'], group['delay_only'], 'on_time'),
                'color_vs_moving_on_time': paired(group['moving_grasp_normalized'], group['moving_grasp'], 'on_time'),
                'full_vs_delay_on_time': paired(group['full'], group['delay_only'], 'on_time'),
                'full_vs_normalized_on_time': paired(group['full'], group['moving_grasp_normalized'], 'on_time'),
            },
        }
    reference = stats['reference']['paired_effects']
    shifted = stats['shifted']
    meets = {
        'reference_on_time_improves_vs_uniform': reference['full_vs_uniform_on_time'][1] > 0,
        'reference_overall_loss_within_10_points': reference['full_vs_native_total'][1] >= -.10,
        'shifted_on_time_improves_vs_delay_only': shifted['paired_effects']['full_vs_delay_on_time'][1] > 0,
        'shifted_on_time_reaches_80_percent': shifted['arms']['full']['on_time'] / shifted['arms']['full']['trials'] >= .8,
        'expanded_on_time_reaches_80_percent': stats['expanded']['arms']['full']['on_time'] / stats['expanded']['arms']['full']['trials'] >= .8,
    }
    return dict(status='observed_current', scope='Reduced simulation. No hardware validation.',
                deadline_seconds=DEADLINE, control_hz=HZ, seeds=SEEDS, training_per_seed=TRAIN,
                holdout_per_seed_per_domain=HOLDOUT, target_checks=meets, results=stats), [
        row for domain in grouped.values() for trials in domain.values() for row in trials
    ]


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)
    results, rows = run()
    (args.output_dir/'followup-results.json').write_text(json.dumps(results, indent=2) + '\n')
    with (args.output_dir/'followup-trials.csv').open('w', newline='') as file:
        writer = csv.DictWriter(file, fieldnames=rows[0].keys())
        writer.writeheader()
        writer.writerows(rows)
    print(json.dumps(results['target_checks'], indent=2))


if __name__ == '__main__':
    main()
