#!/usr/bin/env python3
"""Convert a VeraPulse JSON export to a local LeRobot v3 image dataset.

Tested with lerobot==0.4.4. Actions are joint velocities in radians/second,
not the position commands expected by an unmodified SO-101 policy.
No hardware connection or Hugging Face upload is performed.
"""
import argparse
import base64
import bisect
import io
import json
import math
from pathlib import Path


def finite(value):
    return isinstance(value, (int, float)) and not isinstance(value, bool) and math.isfinite(value)


def validate(data):
    if data.get('format') != 'verapulse-episodes' or data.get('version') != 1:
        raise ValueError('Expected a VeraPulse JSON v1 export.')
    episodes = data.get('episodes')
    if not isinstance(episodes, list) or not episodes:
        raise ValueError('The export contains no episodes.')
    signature = None
    for episode in episodes:
        if episode.get('mode') != 'simulation' or episode.get('schemaVersion') != 1:
            raise ValueError('This adapter currently accepts simulation episodes only.')
        joints = [j['name'] for j in episode['robot']['joints']]
        if not joints or len(joints) != len(set(joints)):
            raise ValueError('Joint names must be unique.')
        cameras = episode.get('cameras', [])
        if len(cameras) > 1 or (cameras and (cameras[0]['id'] != 'overhead' or cameras[0]['width'] != 320 or cameras[0]['height'] != 240)):
            raise ValueError('Only the 320 × 240 simulation overhead camera is supported.')
        camera_spec = {k: v for k, v in cameras[0].items() if k != 'frames'} if cameras else None
        current = (episode['robot']['id'], episode['robot']['revision'], tuple(joints), camera_spec)
        if signature is not None and current != signature:
            raise ValueError('Export episodes with the same robot and camera configuration together.')
        signature = current
        frames = episode['frames']
        if not frames or len(frames) > 6000:
            raise ValueError('Episodes must contain 1–6000 frames.')
        if cameras and len(cameras[0]['frames']) != len(frames):
            raise ValueError('Every state frame must have a matching camera observation.')
        previous = (-1, -1)
        for index, frame in enumerate(frames):
            times = (frame['simulationTime'], frame['elapsedMs'])
            if frame['index'] != index or not all(finite(t) and t >= 0 for t in times) or any(t < p for t, p in zip(times, previous)):
                raise ValueError('Invalid frame indices or non-monotonic clocks.')
            previous = times
            for key in ['positions', 'velocities']:
                if not all(finite(frame['observation']['robot'][key].get(j)) for j in joints):
                    raise ValueError('Missing or invalid robot state.')
            action = frame.get('action')
            if action and (action.get('type') != 'joint_velocity' or action.get('joint') not in joints or not finite(action.get('velocity'))):
                raise ValueError('Unsupported action representation.')
            if cameras:
                image = cameras[0]['frames'][index]
                if image['frameIndex'] != index or image['elapsedMs'] != frame['elapsedMs'] or not image['dataUrl'].startswith('data:image/jpeg;base64,'):
                    raise ValueError('Camera and robot frame timestamps do not match.')
        if frames[-1]['simulationTime'] - frames[0]['simulationTime'] > 300:
            raise ValueError('Episode exceeds five minutes of simulation time.')
    return episodes


def sample_indices(frames, fps):
    """Resample on simulation time, holding the most recent complete observation."""
    times = [f['simulationTime'] for f in frames]
    count = math.floor((times[-1] - times[0]) * fps + 1e-6) + 1
    return [max(0, bisect.bisect_right(times, times[0] + i / fps + 1e-8) - 1) for i in range(count)]


def convert(data, root, repo_id='local/verapulse', fps=20):
    if not 1 <= fps <= 60:
        raise ValueError('FPS must be between 1 and 60.')
    episodes = validate(data)
    if root.exists():
        raise ValueError('Choose a new output directory; existing datasets are never overwritten.')
    import numpy as np
    from PIL import Image
    from lerobot.datasets.lerobot_dataset import LeRobotDataset

    joints = [j['name'] for j in episodes[0]['robot']['joints']]
    features = {
        'observation.state': {'dtype': 'float32', 'shape': (len(joints),), 'names': joints},
        'observation.velocity': {'dtype': 'float32', 'shape': (len(joints),), 'names': joints},
        'action': {'dtype': 'float32', 'shape': (len(joints),), 'names': joints},
        'observation.source_clock': {'dtype': 'float64', 'shape': (2,), 'names': ['elapsed_ms', 'simulation_seconds']},
        'next.done': {'dtype': 'bool', 'shape': (1,), 'names': None},
        'next.success': {'dtype': 'bool', 'shape': (1,), 'names': None},
    }
    camera = bool(episodes[0].get('cameras'))
    if camera:
        features['observation.images.overhead'] = {'dtype': 'image', 'shape': (240, 320, 3), 'names': ['height', 'width', 'channels']}
    dataset = LeRobotDataset.create(repo_id=repo_id, root=root, fps=fps, robot_type=episodes[0]['robot']['id'], features=features, use_videos=False)
    manifest = {'source_format': 'verapulse-episodes-v1', 'action_semantics': 'joint velocity, radians/second; zero means stop',
                'state_units': 'radians', 'resampling': 'previous observation on simulation-time grid', 'fps': fps, 'episodes': []}
    try:
        for episode in episodes:
            indices = sample_indices(episode['frames'], fps)
            for index, source_index in enumerate(indices):
                source = episode['frames'][source_index]
                state = source['observation']['robot']
                action = source.get('action')
                values = np.zeros(len(joints), dtype=np.float32)
                if action:
                    values[joints.index(action['joint'])] = action['velocity']
                frame = {
                    'observation.state': np.array([state['positions'][j] for j in joints], dtype=np.float32),
                    'observation.velocity': np.array([state['velocities'][j] for j in joints], dtype=np.float32),
                    'action': values,
                    'observation.source_clock': np.array([source['elapsedMs'], source['simulationTime']], dtype=np.float64),
                    'next.done': np.array([index == len(indices) - 1], dtype=bool),
                    'next.success': np.array([index == len(indices) - 1 and episode['outcome'] == 'success'], dtype=bool),
                    'task': episode.get('task') or 'Unspecified task',
                }
                if camera:
                    encoded = episode['cameras'][0]['frames'][source_index]['dataUrl'].split(',', 1)[1]
                    with Image.open(io.BytesIO(base64.b64decode(encoded, validate=True))) as image:
                        if image.size != (320, 240) or image.format != 'JPEG':
                            raise ValueError('Invalid camera image dimensions or encoding.')
                        frame['observation.images.overhead'] = np.asarray(image.convert('RGB')).copy()
                dataset.add_frame(frame)
            dataset.save_episode()
            metadata = {k: episode.get(k) for k in ['id', 'startedAt', 'task', 'outcome', 'environment', 'evaluation', 'stopReason', 'commands']}
            metadata['cameras'] = [{k: v for k, v in c.items() if k != 'frames'} for c in episode.get('cameras', [])]
            manifest['episodes'].append(metadata)
    finally:
        dataset.finalize()
    (root / 'meta' / 'verapulse.json').write_text(json.dumps(manifest, indent=2))
    # Exercise the actual reader; successful writes alone are insufficient.
    loaded = LeRobotDataset(repo_id=repo_id, root=root)
    assert loaded.num_episodes == len(episodes)
    for index in [0, len(loaded) - 1]:
        item = loaded[index]
        assert tuple(item['observation.state'].shape) == (len(joints),)
        if camera:
            assert tuple(item['observation.images.overhead'].shape) == (3, 240, 320)
    return loaded


def main():
    parser = argparse.ArgumentParser(description=__doc__)
    parser.add_argument('input', type=Path)
    parser.add_argument('output', type=Path)
    parser.add_argument('--repo-id', default='local/verapulse')
    parser.add_argument('--fps', type=int, default=20)
    args = parser.parse_args()
    if args.input.stat().st_size > 256 * 1024 * 1024:
        parser.error('Split exports larger than 256 MB into individual episodes.')
    result = convert(json.loads(args.input.read_text()), args.output, args.repo_id, args.fps)
    print(f'Validated {result.num_episodes} episodes and {len(result)} frames in {args.output}')


if __name__ == '__main__':
    main()
