#!/usr/bin/env python3
"""Record, fit and roll out a reduced tomato-sorting model. Requires NumPy."""
import argparse
import csv
import json
import math
from pathlib import Path

import numpy as np

HZ = 30
DT = 1 / HZ
SEEDS = (734, 889, 1027)
TRAIN = 50
TEST = 120
# Authored MotionOrigin concept, second tomato, approach 5.23 s to release 12.78 s.
# This deadline is a design reference. An expert hardware benchmark is still needed.
DEADLINE = 7.55
NATIVE = np.array([1.55, .66, 1.05, 2.15, 1.65, 1.30, .70])
SCHEDULES = {
    'native': NATIVE,
    'uniform_2x': NATIVE / 2,
    'phase_aware': np.array([.98, .54, .81, 1.77, 1.02, .94, .57]),
}
PHASES = ('approach', 'grasp', 'lift', 'roll', 'transfer', 'place', 'release')
DOMAINS = ('reference', 'shifted')
# Rotation is scaled to 0.15 model metres/radian for use in the same servo law.
ROTATION_SCALE = .15
PIXELS = (np.arange(24) + .5) / 12 - 1
GRID_X, GRID_Y = np.meshgrid(PIXELS, PIXELS)
DETAIL_PIXELS = (np.arange(40) + .5) / 20 - 1
DETAIL_X, DETAIL_Y = np.meshgrid(DETAIL_PIXELS, DETAIL_PIXELS)


def scenarios(rng, n, domain):
    shifted = domain == 'shifted'
    defect = np.tile([False, True], n // 2)
    rng.shuffle(defect)
    return {
        'defect': defect,
        'x': rng.uniform(-.060, .060, n),
        'radius': rng.uniform(.027, .039, n),
        'belt': rng.uniform(.035, .070, n) if shifted else rng.uniform(.035, .055, n),
        'angle': rng.uniform(-np.pi, np.pi, n),
        'latitude': rng.uniform(-.65, .65, n) if shifted else rng.uniform(-.48, .48, n),
        'scar': rng.uniform(.12, .25, n) if shifted else rng.uniform(.15, .25, n),
        'light': rng.uniform(.46, 1.12, n) if shifted else rng.uniform(.78, 1.14, n),
        'shadow': rng.uniform(.10, .50, n) if shifted else rng.uniform(.05, .26, n),
        'tint': rng.uniform(.80, 1.23, n) if shifted else rng.uniform(.90, 1.10, n),
        'tau': rng.uniform(.115, .205, n) if shifted else rng.uniform(.070, .145, n),
        'delay': rng.integers(3, 7, n) if shifted else rng.integers(1, 4, n),
        'speed': rng.uniform(.37, .64, (n, 3)),
        'offset': rng.normal(0, .006 if shifted else .003, (n, 2)),
    }


def inspect_frame(cases, rotation, camera, rng, normalize_color=False, detail=False):
    """Rasterized RGB crop of a marked sphere, with simple light and occlusion."""
    scale = np.clip(cases['radius'] / .046, .60, .85)[:, None, None]
    grid_x, grid_y = (DETAIL_X, DETAIL_Y) if detail else (GRID_X, GRID_Y)
    x, y = grid_x / scale, grid_y / scale
    z_sq = 1 - x * x - y * y
    fruit = z_sq > 0
    z = np.sqrt(np.maximum(z_sq, 0))
    angle = cases['angle'] + rotation / ROTATION_SCALE + camera + cases.get('camera_bias', 0)
    lat = cases['latitude']
    toward_scar = (
        x * (np.cos(lat) * np.sin(angle))[:, None, None]
        + y * np.sin(lat)[:, None, None]
        + z * (np.cos(lat) * np.cos(angle))[:, None, None]
    )
    blemish = fruit & (toward_scar > np.cos(cases['scar'])[:, None, None]) & cases['defect'][:, None, None]
    shading = (.56 + .44 * z - .20 * x) * (1 - cases['shadow'][:, None, None] * (y > .25))
    # A highlight and darker bottom edge make a single absolute colour threshold unreliable.
    glare = .16 * np.exp(-((x+.30)**2 + (y+.28)**2) / .035)
    color = np.where(blemish[..., None], [.29, .145, .075], [.77, .105, .052])
    rgb = (color * shading[..., None] + glare[..., None]) * cases['light'][:, None, None, None]
    rgb[..., 1] *= cases['tint'][:, None, None]
    rgb += rng.normal(0, .012, rgb.shape)
    rgb = np.where(fruit[..., None], rgb, [.11, .22, .20])
    if 'occlusion' in cases:
        side = -1 if camera else 1
        edge = (side * x + .30 * y > 1 - cases['occlusion'][:, None, None]) & fruit
        rgb[edge] = [.14, .16, .17]
    # Crop masking uses colour and position only. No ground-truth scar/fruit mask reaches the learner.
    red, green, blue = np.moveaxis(rgb, -1, 0)
    if normalize_color:
        # Blemish evidence is a darker, locally coherent patch with a different
        # chromaticity from the body of this fruit. Neutral shadows alone should
        # not trigger. The known object crop remains an assumption of the model.
        interior = (x*x + y*y < .78) & (red > .085)
        area = interior.sum(axis=(1, 2)).clip(1)
        green_ratio = np.maximum(green, .004) / np.maximum(red, .035)
        blue_ratio = np.maximum(blue, .004) / np.maximum(red, .035)
        def relative(ratio):
            masked = np.ma.array(ratio, mask=~interior).reshape(len(red), -1)
            typical = np.ma.median(masked, axis=1).filled(.1)
            return ratio / np.maximum(typical, .02)[:, None, None]
        relative_green = relative(green_ratio)
        relative_blue = relative(blue_ratio)
        candidates = interior & (relative_green > 1.85) & (relative_blue > 1.5)
        # The local deviation of green and blue ratios must agree with the
        # body's green/blue balance. Neutral glare instead dilutes that balance.
        candidates &= (relative_green > .82 * relative_blue) & (red > 1.10 * green)
        support = (candidates[:, :-1, :-1].astype(np.uint8) + candidates[:, 1:, :-1] +
                   candidates[:, :-1, 1:] + candidates[:, 1:, 1:])
        coherent = (support >= 3).sum(axis=(1, 2))
        saturation = 22 if detail else 8
        return np.column_stack((coherent / area, coherent.clip(0, saturation) / (3*saturation)))
    crop = (red > green * 1.22) & (red > .10) & ((x*x+y*y) < .85)
    usable = crop.sum(axis=(1, 2)).clip(1)
    ratio = green / np.maximum(red, .05)
    dark_red = (red < .58 * np.max(np.where(crop, red, 0), axis=(1, 2))[:, None, None])
    return np.column_stack(((crop & dark_red & (ratio > .29)).sum(axis=(1, 2)) / usable,
                            np.max(np.where(crop & dark_red, ratio - blue / np.maximum(red, .05), 0), axis=(1, 2))))


def path(cases, duration, route, phase, roll_degrees=155):
    """Desired reference segment in x, height and scaled rotation."""
    n = len(route)
    zero = np.zeros(n)
    height = cases['radius']
    start_x = cases['x'] - cases['belt'] * sum(duration[:2])
    observed_x = start_x + cases['offset'][:, 0]
    observed_height = height + cases['offset'][:, 1]
    # Negative x is downstream; positive x is the yellow reject tray.
    destination_x = np.where(route, .184, -.242)
    approach = np.column_stack((observed_x, observed_height, zero))
    lifted = np.column_stack((observed_x, height + .124, zero))
    rolled = lifted.copy()
    rolled[:, 2] = math.radians(roll_degrees) * ROTATION_SCALE
    above = np.column_stack((destination_x, height + .115, rolled[:, 2]))
    floor = above.copy()
    floor[:, 1] = height
    points = [np.tile([.13, .185, 0], (n, 1)), approach, approach, lifted, rolled, above, floor, floor]
    return points[phase], points[phase+1]


def design(error, feed):
    return np.stack((error, feed, np.tanh(error / .035) * .035), axis=-1)


def teacher(error, feed):
    return feed + 5.4 * error + .10 * np.tanh(error / .035)


def fit(samples):
    x = np.concatenate([a for a, _ in samples]).reshape(-1, 3)
    y = np.concatenate([b for _, b in samples]).ravel()
    return np.linalg.solve(x.T @ x + .012 * np.eye(x.shape[1]), x.T @ y)


def fit_sorter(stats, truth):
    x = np.column_stack((np.ones(len(stats)), stats * 12))
    weights = np.zeros(x.shape[1])
    for _ in range(16):
        probability = 1 / (1 + np.exp(-np.clip(x @ weights, -24, 24)))
        penalty = np.diag([.01] + [.18] * (x.shape[1] - 1))
        gradient = x.T @ (truth - probability) - penalty @ weights
        hessian = (x.T * (probability * (1 - probability))) @ x + penalty
        weights += np.linalg.solve(hessian, gradient)
    return weights


def rollout(cases, duration, rng, controller=None, classifier=None, compensate=False, *,
            track_belt=False, normalize_color=False, roll_degrees=155, detail=False, route_cutoff=0):
    """Execute a closed-loop rollout with state lag and settled-contact gates."""
    teaching = controller is None
    truth = cases['defect']
    n = len(truth)
    route = truth.copy() if teaching else np.zeros(n, dtype=bool)
    route_score = np.zeros(n)
    q = np.tile([.13, .185, 0.], (n, 1))
    velocity = np.zeros_like(q)
    lag = np.repeat(q[None, ...], 8, axis=0)
    movement = np.arange(n)
    usable = np.ones(n, dtype=bool)
    reasons = np.full(n, 'success', dtype='<U25')
    settled = np.zeros(n)
    time = np.zeros(n)
    phase_boundaries = [time.copy()]
    stats = np.zeros((n, 4))
    recorded = []
    travel = np.zeros(n)
    placement_error = np.full(n, np.nan)
    belt_accel = cases.get('belt_accel', 0)

    def fruit_x(elapsed):
        return cases['x'] - (cases['belt'] + .5 * belt_accel * elapsed) * elapsed

    if track_belt:
        # One camera position observation and a calibrated but noisy belt encoder.
        compensated_commands = np.zeros_like(lag)
        speed_history = np.zeros_like(lag)
        lift_x = q[:, 0].copy()
        truth_dwell = np.zeros(n)
        estimated_fruit_x = cases['x'] + cases['offset'][:, 0]

    def fail(failed, reason):
        first = failed & usable
        usable[first] = False
        reasons[first] = reason

    for phase, seconds in enumerate(duration):
        start, goal = path(cases, duration, route, phase, roll_degrees)
        if track_belt and phase >= 2:
            if phase in (2, 3, 4):
                start[:, 0] = lift_x
            if phase in (2, 3):
                goal[:, 0] = lift_x
        displacement = goal - start
        elapsed = np.zeros(n)
        settled[:] = 0
        if track_belt:
            truth_dwell[:] = 0
        frames = math.ceil(seconds * HZ)
        sample_frames = set(np.linspace(0, frames - 1, 10, dtype=int))
        # Same short feedback window in every condition. Early finish occurs only
        # after that episode satisfies the same hold and endpoint conditions.
        settle_need = .16 if phase == 1 else (.28 if phase == 6 else 0)
        extra = 8
        active = np.ones(n, dtype=bool)
        for frame in range(frames + extra):
            remaining = np.maximum(0, seconds - elapsed)
            u = np.minimum(1, elapsed / seconds)[:, None]
            ref = start + (u*u*(3-2*u)) * displacement
            feed = (6*u*(1-u)) * displacement / seconds
            if track_belt and phase in (0, 1):
                # Model a belt encoder with a two-tick transport delay and a
                # fixed calibration error. Integrate its readings on each case clock.
                measured_belt = cases['measured_belt'] + belt_accel * np.maximum(0, time - 2*DT)
                progress = (u*u*(3-2*u)).ravel() if phase == 0 else np.ones(n)
                change = (6*u*(1-u)).ravel()/seconds if phase == 0 else np.zeros(n)
                goal[:, 0] = estimated_fruit_x
                delta = goal[:,0] - start[:,0] if phase == 0 else np.zeros(n)
                ref[:,0] = start[:,0] + progress * delta if phase == 0 else goal[:,0]
                feed[:,0] = change * delta - progress * measured_belt
            observed = lag[cases['delay'], movement]
            if track_belt:
                # Advance the last available position and velocity through issued
                # commands with a single calibrated actuator time constant.
                observed_speed = speed_history[cases['delay'], movement]
                for slot in range(6, 0, -1):
                    known = cases['delay'] >= slot
                    observed_speed[known] += (DT/.137) * (compensated_commands[slot, known] - observed_speed[known])
                    observed[known] += observed_speed[known] * DT
            elif compensate:
                previous = lag[cases['delay'] + 1, movement]
                # Estimate the current encoder state from the two latest available samples.
                # The correction uses only measurements up to the delayed observation.
                motion = np.clip(observed - previous, -cases['speed'] * DT, cases['speed'] * DT)
                observed = observed + motion * cases['delay'][:, None]
            error = ref - observed
            features = design(error, feed)
            requested = teacher(error, feed) if teaching else features @ controller
            command = np.clip(requested, -cases['speed'], cases['speed'])
            velocity[active] += (DT / cases['tau'][active, None]) * (command[active] - velocity[active])
            q[active] += velocity[active] * DT
            # Keep each episode's latency buffer on its own control clock,
            # including when another member of the batch needs extra time.
            lag[1:, active] = lag[:-1, active]
            lag[0, active] = q[active]
            if track_belt:
                compensated_commands[1:, active] = compensated_commands[:-1, active]
                compensated_commands[1, active] = command[active]
                speed_history[1:, active] = speed_history[:-1, active]
                speed_history[0, active] = velocity[active]
            elapsed[active] += DT
            time[active] += DT
            if track_belt and phase in (0, 1):
                estimated_fruit_x[active] -= measured_belt[active] * DT
            if teaching and frame in sample_frames:
                recorded.append((features, command))
            if phase == 3 and (frame % 5 == 0 or frame == frames - 1):
                both = np.column_stack([inspect_frame(cases, q[:,2], angle, rng, normalize_color, detail)
                                        for angle in (0., -1.15)])
                stats[active] = np.maximum(stats[active], both[active])
            if phase in (1, 6):
                remaining_pos = np.linalg.norm((q - goal)[:, :2], axis=1)
                stable = (remaining_pos <= (.014 if phase == 1 else .012)) & (np.linalg.norm(velocity[:, :2],axis=1) <= .060)
                if track_belt:
                    # Gate with the estimated arm. Score sustained alignment
                    # against the simulated state independently.
                    predicted_next = observed + observed_speed * DT
                    target = goal[:, :2].copy()
                    relative_speed = observed_speed[:, :2].copy()
                    if phase == 1:
                        target[:, 0] -= measured_belt * DT
                        relative_speed[:, 0] += measured_belt
                        true_x = fruit_x(time)
                        relative_error = np.hypot(q[:,0] - true_x, q[:,1] - cases['radius'])
                        actual_speed = np.hypot(velocity[:,0] + cases['belt'] + belt_accel*time, velocity[:,1])
                        true_stable = (relative_error <= .024) & (actual_speed <= .060)
                        stable = np.linalg.norm(predicted_next[:, :2] - target, axis=1) <= .014
                    else:
                        true_stable = (remaining_pos <= .012) & (np.linalg.norm(velocity[:, :2], axis=1) <= .060)
                        stable = np.linalg.norm(predicted_next[:, :2] - target, axis=1) <= .010
                    stable &= np.linalg.norm(relative_speed, axis=1) <= .060
                    truth_dwell[active & ~true_stable] = 0
                    truth_dwell[active & true_stable] += DT
                settled[~stable] = 0
                settled[stable & active] += DT
                ready = settled >= settle_need
            else:
                current = observed + observed_speed * DT if track_belt else q
                extent = np.abs(current - goal)
                ready = extent[:, :2].max(axis=1) <= .025
                if phase == 3:
                    ready &= extent[:, 2] <= .10
            active &= (remaining > DT * .1) | ~ready
            if not active.any():
                break

        if phase == 1:
            # Drift between the planned intercept and actual closure is a modelled belt penalty.
            true_x = fruit_x(time)
            grasp_error = np.hypot(q[:,0] - true_x, q[:,1] - cases['radius'])
            fail((settled < settle_need) | (grasp_error > .024), 'missed_grasp')
            if track_belt:
                fail(truth_dwell < settle_need, 'missed_grasp')
                lift_x = observed[:,0].copy()
        if phase == 3:
            travel = q[:, 2] / ROTATION_SCALE
            fail(travel < math.radians(123), 'insufficient_roll')
            if not teaching:
                feat = stats if len(classifier) == 5 else stats[:, :2]
                route_score = np.column_stack((np.ones(n), feat * 12)) @ classifier
                route = route_score >= route_cutoff
            inspected = usable.copy()
        if phase == 6:
            placement_error = np.hypot(q[:,0] - goal[:,0], q[:,1] - goal[:,1])
            fail((settled < settle_need) | (placement_error > .012), 'unsettled_release')
            if track_belt:
                fail(truth_dwell < settle_need, 'unsettled_release')
            fail(route != truth, 'wrong_route')
        phase_boundaries.append(time.copy())
    return dict(samples=recorded, stats=stats, route=route, route_score=route_score,
                inspected=inspected, reasons=reasons, success=usable,
                seconds=time, placement_error=placement_error, roll_deg=np.degrees(travel),
                phase_boundaries=np.stack(phase_boundaries, axis=1))


def aggregate(group):
    ok = np.array([r['success'] for r in group])
    elapsed = np.array([r['seconds'] for r in group])
    inspected = [r for r in group if r['inspected']]
    rejected = [r for r in inspected if r['defective']]
    return dict(
        trials=len(group), successes=int(ok.sum()), on_time=int((ok & (elapsed <= DEADLINE)).sum()),
        median_success_seconds=round(float(np.median(elapsed[ok])), 3) if ok.any() else None,
        inspected=len(inspected), correct_route=sum(r['correct_route'] for r in inspected),
        defective_correct_route=sum(r['correct_route'] for r in rejected), defective_total=len(rejected),
        failures={reason:sum(r['outcome'] == reason for r in group)
                  for reason in ('missed_grasp', 'insufficient_roll', 'unsettled_release', 'wrong_route')},
    )


def paired_interval(a, b):
    diff = np.array([int(x['on_time']) - int(y['on_time']) for x, y in zip(a, b)])
    est = []
    rng = np.random.default_rng(28790)
    for _ in range(2500):
        # Match cases and resample independently inside each fixed training seed.
        samples = [rng.choice(block, len(block), replace=True) for block in np.split(diff, len(SEEDS))]
        est.append(np.mean(samples))
    return [round(float(v), 4) for v in [diff.mean(), *np.quantile(est, [.025, .975])]]


def run():
    rows = []
    fits = {}
    def log(seed, domain, name, cameras, holdout, result):
        for j in range(TEST):
            inspected = bool(result['inspected'][j])
            rows.append(dict(seed=seed, case=j, domain=domain, schedule=name, cameras=cameras,
                             defective=bool(holdout['defect'][j]), inspected=inspected,
                             correct_route=bool(result['route'][j] == holdout['defect'][j]) if inspected else None,
                             success=bool(result['success'][j]), on_time=bool(result['success'][j] and result['seconds'][j] <= DEADLINE),
                             seconds=round(float(result['seconds'][j]), 6), roll_deg=round(float(result['roll_deg'][j]), 3),
                             placement_error_mm=round(float(result['placement_error'][j]*1000), 3), outcome=str(result['reasons'][j])))
    for seed in SEEDS:
        training = scenarios(np.random.default_rng(seed), TRAIN, 'reference')
        teacher_run = rollout(training, NATIVE, np.random.default_rng(seed + 1))
        full = fit_sorter(teacher_run['stats'], training['defect'])
        close = fit_sorter(teacher_run['stats'][:, :2], training['defect'])
        fits[str(seed)] = {}
        for name, duration in SCHEDULES.items():
            training_roll = teacher_run if name == 'native' else rollout(training, duration, np.random.default_rng(seed + 1))
            controller = fit(training_roll['samples'])
            fits[str(seed)][name] = [round(float(v), 6) for v in controller]
            for domain_no, domain in enumerate(DOMAINS):
                holdout = scenarios(np.random.default_rng(seed + 10000 + domain_no), TEST, domain)
                for cameras, classifier in [('dual', full), ('head_only', close)]:
                    if cameras == 'head_only' and name != 'phase_aware':
                        continue
                    result = rollout(holdout, duration, np.random.default_rng(seed + 20000 + domain_no), controller, classifier)
                    log(seed, domain, name, cameras, holdout, result)
            if name == 'phase_aware':
                holdout = scenarios(np.random.default_rng(seed + 30000), TEST, 'shifted')
                for version, compensate in [('phase_aware', False), ('latency_compensated', True)]:
                    result = rollout(holdout, duration, np.random.default_rng(seed + 40000), controller, full, compensate)
                    log(seed, 'shifted_followup', version, 'dual', holdout, result)
    def get(domain, schedule, cameras):
        return [r for r in rows if (r['domain'],r['schedule'],r['cameras']) == (domain,schedule,cameras)]
    summary = {}
    for domain in DOMAINS:
        group = {n: get(domain,n,'dual') for n in SCHEDULES}
        group['phase_aware_head_only'] = get(domain,'phase_aware','head_only')
        assert all([(r['seed'], r['case']) for r in g] == [(r['seed'], r['case']) for r in group['native']] for g in group.values())
        summary[domain] = dict(arms={n: aggregate(g) for n,g in group.items()},
                               paired_gain={n:paired_interval(group['phase_aware'],group[n]) for n in ('native','uniform_2x','phase_aware_head_only')})
    followup = {name:get('shifted_followup',name,'dual') for name in ('phase_aware', 'latency_compensated')}
    summary['shifted_followup'] = dict(
        arms={name:aggregate(group) for name,group in followup.items()},
        paired_gain={'phase_aware':paired_interval(followup['latency_compensated'],followup['phase_aware'])},
    )
    return dict(
        status='observed_current', scope='Reduced simulation, with authored task geometry and robot-aligned RGB crops. No hardware calibration.',
        seeds=SEEDS, train_episodes_per_seed=TRAIN, holdout_episodes_per_seed_per_domain=TEST, control_hz=HZ,
        deadline_seconds=DEADLINE, phases=PHASES,
        schedules={k:[float(x) for x in v] for k,v in SCHEDULES.items()}, fit_coefficients=fits, results=summary,
    ), rows


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


if __name__ == '__main__':
    main()
