import h5py
import sys

def read_sixdof_h5_file(file_path):
    """
    Read and print six dof data from HDF5 file
    Includes frame sequence output and statistical information
    """
    frame_count = 0
    first_index = None
    last_index = None
    elemenets_per_frame = 0

    try:
        # Open HDF5 file
        with h5py.File(file_path, 'r') as h5_file:
            # Check if dataset exists
            if 'data' not in h5_file:
                print(f"Error: 'data' dataset not found in file - {file_path}")
                return
            
            dataset = h5_file['data']
            frame_count = len(dataset)
            print(f"Successfully opened file: {file_path}")
            print(f"File contains {frame_count} frames of data\n")
            
            # Iterate through each frame of data
            for frame_seq, frame in enumerate(dataset, start=0):
                # Record first and last frame index
                if first_index is None:
                    first_index = frame['index']
                last_index = frame['index']
                
                parts_info = []
                for element in frame['elements']:
                    # Ensure name is properly decoded
                    if isinstance(element['name'], bytes):
                        name = element['name'].decode('utf-8', errors='replace')
                    else:
                        name = str(element['name'])
                    
                    # Format data
                    pos = element['position']
                    rot = element['rotation']
                    parts_info.append(
                        f"{name}(pos: [{pos[0]:.3f}, {pos[1]:.3f}, {pos[2]:.3f}], "
                        f"rot: [{rot[0]:.3f}, {rot[1]:.3f}, {rot[2]:.3f}, {rot[3]:.3f}])"
                    )
                
                if frame_seq == 1:
                    elemenets_per_frame = len(parts_info)
                
                data_str = (f"[{frame_seq}] 6DOF, {frame['index']}, {frame['timestamp']:.3f}: {', '.join(parts_info)}")
                print(data_str)
        
        # Print statistical information
        print("\n" + "="*80)
        print("Summary:")
        print(f"  File Path: {file_path}")
        print(f"  Total frames: {frame_count}")
        print(f"  Elements per frame: {elemenets_per_frame}")
        
        # Calculate and display index difference
        if frame_count > 0 and first_index is not None and last_index is not None:
            index_diff = (last_index + 1) - first_index
            print(f"  Index range difference: (last_index + 1) - first_index = ({last_index} + 1) - {first_index} = {index_diff}")
            print(f"  Index continuity: {'Normal' if index_diff == frame_count else 'Abnormal'}")
        else:
            print("  Index range difference: Cannot calculate (no valid data)")
        print("="*80)

    except FileNotFoundError:
        print(f"Error: File not found - {file_path}")
    except PermissionError:
        print(f"Error: No permission to access file - {file_path}")
    except Exception as e:
        print(f"Error occurred while reading file: {str(e)}")

if __name__ == "__main__":
    import sys
    
    if len(sys.argv) != 2:
        print("Usage: python read_sixdof_data_h5.py <H5 file path>")
        sys.exit(1)
    
    file_path = sys.argv[1]
    read_sixdof_h5_file(file_path)