parser.go

package ulogo

import (
	"bytes"
	"encoding/binary"
	"errors"
	"fmt"
	"io"
	"os"
	"sort"
	"strconv"
	"strings"
)

// Option changes ULog parsing behavior.
type Option func(*loadOptions)

type loadOptions struct {
	messageFilter map[string]struct{}
	headerOnly    bool
}

// WithMessageFilter loads data streams only for the given topic names.
// A nil filter loads every stream. An empty filter loads definitions only.
func WithMessageFilter(names []string) Option {
	return func(opts *loadOptions) {
		if names == nil {
			opts.messageFilter = nil
			return
		}
		opts.messageFilter = make(map[string]struct{}, len(names))
		for _, name := range names {
			opts.messageFilter[name] = struct{}{}
		}
	}
}

// HeaderOnly stops after the definition section.
func HeaderOnly() Option {
	return func(opts *loadOptions) {
		opts.headerOnly = true
	}
}

// Load opens and parses a ULog file.
func Load(filename string, options ...Option) (*ULog, error) {
	return ReadFile(filename, options...)
}

// ReadFile opens and parses a ULog file.
func ReadFile(filename string, options ...Option) (*ULog, error) {
	file, err := os.Open(filename)
	if err != nil {
		return nil, err
	}
	defer file.Close()
	return Read(file, options...)
}

// Read parses ULog data from reader.
//
// If reader implements io.ReadSeeker, Read parses directly from it. Otherwise
// Read buffers the input in memory and parses from that buffer, which keeps the
// API usable for network, archive, and in-memory inputs without writing a
// temporary file. ULog appended data requires seeking, so truly streaming
// parsing is exposed separately from the full in-memory ULog parser.
func Read(reader io.Reader, options ...Option) (*ULog, error) {
	if reader == nil {
		return nil, fmt.Errorf("nil reader")
	}
	if seeker, ok := reader.(io.ReadSeeker); ok {
		return readSeek(seeker, options...)
	}
	raw, err := io.ReadAll(reader)
	if err != nil {
		return nil, err
	}
	return readSeek(bytes.NewReader(raw), options...)
}

func readSeek(reader io.ReadSeeker, options ...Option) (*ULog, error) {
	opts := loadOptions{}
	for _, option := range options {
		option(&opts)
	}

	log := newULog()
	p := parser{
		reader:        reader,
		log:           log,
		opts:          opts,
		subscriptions: map[uint16]*subscription{},
		filteredIDs:   map[uint16]struct{}{},
		missingIDs:    map[uint16]struct{}{},
	}

	if err := p.readHeader(); err != nil {
		return nil, err
	}
	log.LastTimestamp = log.StartTimestamp

	if err := p.readDefinitions(); err != nil {
		return nil, err
	}
	if opts.headerOnly {
		return log, nil
	}

	if log.HasDataAppended() && len(log.AppendedOffsets) > 0 {
		for _, offset := range log.AppendedOffsets {
			if err := p.readData(int64(offset)); err != nil {
				return nil, err
			}
			if _, err := p.reader.Seek(int64(offset), io.SeekStart); err != nil {
				return nil, err
			}
		}
	}
	if err := p.readData(1 << 62); err != nil {
		return nil, err
	}
	p.flushDatasets()

	sort.Slice(log.Data, func(i, j int) bool {
		if log.Data[i].Name == log.Data[j].Name {
			return log.Data[i].MultiID < log.Data[j].MultiID
		}
		return log.Data[i].Name < log.Data[j].Name
	})

	return log, nil
}

type parser struct {
	reader        io.ReadSeeker
	log           *ULog
	opts          loadOptions
	subscriptions map[uint16]*subscription
	filteredIDs   map[uint16]struct{}
	missingIDs    map[uint16]struct{}
}

type messageHeader struct {
	size uint16
	typ  byte
}

type subscription struct {
	multiID         byte
	msgID           uint16
	messageName     string
	fields          []FlatField
	timestampIndex  int
	timestampOffset int
	minDataSize     int
	rows            []Row
}

func (p *parser) readHeader() error {
	header := make([]byte, 16)
	if _, err := io.ReadFull(p.reader, header); err != nil {
		if errors.Is(err, io.ErrUnexpectedEOF) || errors.Is(err, io.EOF) {
			return fmt.Errorf("invalid ULog file: header too short")
		}
		return err
	}
	if !bytes.Equal(header[:7], HeaderBytes) {
		return fmt.Errorf("invalid ULog file: bad header magic")
	}
	p.log.Version = header[7]
	p.log.StartTimestamp = binary.LittleEndian.Uint64(header[8:16])
	return nil
}

func (p *parser) readDefinitions() error {
	for {
		header, err := p.readMessageHeader()
		if err != nil {
			if errors.Is(err, io.EOF) {
				return nil
			}
			return err
		}
		data, err := p.readMessageData(header)
		if err != nil {
			return err
		}

		switch header.typ {
		case msgTypeInfo:
			info, err := parseInfo(data, false)
			if err != nil {
				p.log.FileCorruption = true
				continue
			}
			p.log.Info[info.key] = info.value
			p.log.InfoTypes[info.key] = info.value.Type
		case msgTypeInfoMultiple:
			info, err := parseInfo(data, true)
			if err != nil {
				p.log.FileCorruption = true
				continue
			}
			p.addInfoMultiple(info)
		case msgTypeFormat:
			format, err := parseFormat(data)
			if err != nil {
				p.log.FileCorruption = true
				continue
			}
			p.log.MessageFormats[format.Name] = format
		case msgTypeParameter:
			param, err := parseInfo(data, false)
			if err != nil {
				p.log.FileCorruption = true
				continue
			}
			p.log.InitialParameters[param.key] = param.value
		case msgTypeParameterDefault:
			if err := p.addDefaultParameter(data); err != nil {
				p.log.FileCorruption = true
			}
		case msgTypeFlagBits:
			p.parseFlags(data)
		case msgTypeAddLogged, msgTypeLogging, msgTypeLoggingTagged:
			_, _ = p.reader.Seek(-int64(3+header.size), io.SeekCurrent)
			return nil
		default:
			if packetLooksCorrupt(header) {
				p.log.FileCorruption = true
				_, _ = p.reader.Seek(-int64(2+header.size), io.SeekCurrent)
			}
		}
	}
}

func (p *parser) readData(readUntil int64) error {
	for {
		start, _ := p.reader.Seek(0, io.SeekCurrent)
		header, err := p.readMessageHeader()
		if err != nil {
			if errors.Is(err, io.EOF) {
				return nil
			}
			return err
		}
		data, err := p.readMessageData(header)
		if err != nil {
			if errors.Is(err, io.ErrUnexpectedEOF) {
				return nil
			}
			return err
		}
		end, _ := p.reader.Seek(0, io.SeekCurrent)
		if end > readUntil {
			_, _ = p.reader.Seek(start, io.SeekStart)
			return nil
		}

		switch header.typ {
		case msgTypeInfo:
			info, err := parseInfo(data, false)
			if err != nil {
				p.log.FileCorruption = true
				continue
			}
			p.log.Info[info.key] = info.value
			p.log.InfoTypes[info.key] = info.value.Type
		case msgTypeInfoMultiple:
			info, err := parseInfo(data, true)
			if err != nil {
				p.log.FileCorruption = true
				continue
			}
			p.addInfoMultiple(info)
		case msgTypeParameter:
			param, err := parseInfo(data, false)
			if err != nil {
				p.log.FileCorruption = true
				continue
			}
			p.log.ChangedParameters = append(p.log.ChangedParameters, ChangedParameter{
				Timestamp: p.log.LastTimestamp,
				Name:      param.key,
				Value:     param.value,
			})
		case msgTypeParameterDefault:
			if err := p.addDefaultParameter(data); err != nil {
				p.log.FileCorruption = true
			}
		case msgTypeAddLogged:
			sub, err := p.parseAddLogged(data)
			if err != nil {
				p.log.FileCorruption = true
				continue
			}
			if p.acceptsMessage(sub.messageName) {
				p.subscriptions[sub.msgID] = sub
			} else {
				p.filteredIDs[sub.msgID] = struct{}{}
			}
		case msgTypeLogging:
			msg, err := parseLogging(data)
			if err != nil {
				p.log.FileCorruption = true
				continue
			}
			p.log.LoggedMessages = append(p.log.LoggedMessages, msg)
		case msgTypeLoggingTagged:
			msg, err := parseTaggedLogging(data)
			if err != nil {
				p.log.FileCorruption = true
				continue
			}
			p.log.LoggedMessagesTagged[msg.Tag] = append(p.log.LoggedMessagesTagged[msg.Tag], msg)
		case msgTypeData:
			timestamp, err := p.parseData(data)
			if err != nil {
				p.log.FileCorruption = true
				continue
			}
			if timestamp > p.log.LastTimestamp {
				p.log.LastTimestamp = timestamp
			}
		case msgTypeDropout:
			if len(data) >= 2 {
				p.log.Dropouts = append(p.log.Dropouts, Dropout{
					Timestamp: p.log.LastTimestamp,
					Duration:  binary.LittleEndian.Uint16(data[:2]),
				})
			}
		case msgTypeSync:
			p.log.SyncCount++
		case msgTypeRemoveLogged:
			// pyulog currently keeps already decoded data and ignores remove messages.
		default:
			if packetLooksCorrupt(header) {
				p.log.FileCorruption = true
			}
		}
	}
}

func (p *parser) readMessageHeader() (messageHeader, error) {
	var buf [3]byte
	n, err := io.ReadFull(p.reader, buf[:])
	if err != nil {
		if errors.Is(err, io.EOF) || (errors.Is(err, io.ErrUnexpectedEOF) && n == 0) {
			return messageHeader{}, io.EOF
		}
		return messageHeader{}, err
	}
	return messageHeader{size: binary.LittleEndian.Uint16(buf[:2]), typ: buf[2]}, nil
}

func (p *parser) readMessageData(header messageHeader) ([]byte, error) {
	data := make([]byte, int(header.size))
	_, err := io.ReadFull(p.reader, data)
	return data, err
}

func (p *parser) parseFlags(data []byte) {
	if len(data) < 40 {
		p.log.FileCorruption = true
		return
	}
	copy(p.log.CompatFlags[:], data[:8])
	copy(p.log.IncompatFlags[:], data[8:16])
	p.log.AppendedOffsets = p.log.AppendedOffsets[:0]
	for i := 0; i < 3; i++ {
		offset := binary.LittleEndian.Uint64(data[16+i*8 : 24+i*8])
		if offset != 0 {
			p.log.AppendedOffsets = append(p.log.AppendedOffsets, offset)
		}
	}
}

func (p *parser) addInfoMultiple(info parsedInfo) {
	if _, ok := p.log.InfoMultiple[info.key]; !ok {
		p.log.InfoMultiple[info.key] = [][]Value{{info.value}}
		p.log.InfoMultipleTypes[info.key] = info.value.Type
		return
	}
	if info.continued && len(p.log.InfoMultiple[info.key]) > 0 {
		last := len(p.log.InfoMultiple[info.key]) - 1
		p.log.InfoMultiple[info.key][last] = append(p.log.InfoMultiple[info.key][last], info.value)
		return
	}
	p.log.InfoMultiple[info.key] = append(p.log.InfoMultiple[info.key], []Value{info.value})
}

func (p *parser) addDefaultParameter(data []byte) error {
	if len(data) < 2 {
		return fmt.Errorf("parameter default too short")
	}
	defaultTypes := data[0]
	info, err := parseInfo(data[1:], false)
	if err != nil {
		return err
	}
	for defaultTypes != 0 {
		bit := defaultTypes & -defaultTypes
		defaultTypes ^= bit
		defaultType := int(bit - 1)
		if p.log.DefaultParameters[defaultType] == nil {
			p.log.DefaultParameters[defaultType] = map[string]Value{}
		}
		p.log.DefaultParameters[defaultType][info.key] = info.value
	}
	return nil
}

func (p *parser) parseAddLogged(data []byte) (*subscription, error) {
	if len(data) < 3 {
		return nil, fmt.Errorf("add logged message too short")
	}
	sub := &subscription{
		multiID:        data[0],
		msgID:          binary.LittleEndian.Uint16(data[1:3]),
		messageName:    string(data[3:]),
		timestampIndex: -1,
	}
	if err := p.flattenFields("", sub.messageName, &sub.fields); err != nil {
		return nil, err
	}
	for len(sub.fields) > 0 && strings.HasPrefix(sub.fields[len(sub.fields)-1].Name, "_padding") {
		sub.fields = sub.fields[:len(sub.fields)-1]
	}
	offset := 0
	for i := range sub.fields {
		sub.fields[i].Offset = offset
		if sub.fields[i].Name == "timestamp" {
			sub.timestampIndex = i
			sub.timestampOffset = offset
		}
		offset += sub.fields[i].Size
	}
	sub.minDataSize = offset
	return sub, nil
}

func (p *parser) flattenFields(prefix, typeName string, out *[]FlatField) error {
	format, ok := p.log.MessageFormats[typeName]
	if !ok {
		return fmt.Errorf("missing message format for %q", typeName)
	}
	for _, field := range format.Fields {
		if isPrimitive(field.Type) {
			size, _ := FieldSize(field.Type)
			if field.ArraySize > 0 {
				for i := 0; i < field.ArraySize; i++ {
					*out = append(*out, FlatField{
						Name: prefix + field.Name + "[" + strconv.Itoa(i) + "]",
						Type: field.Type,
						Size: size,
					})
				}
				continue
			}
			*out = append(*out, FlatField{
				Name: prefix + field.Name,
				Type: field.Type,
				Size: size,
			})
			continue
		}

		if field.ArraySize > 0 {
			for i := 0; i < field.ArraySize; i++ {
				if err := p.flattenFields(prefix+field.Name+"["+strconv.Itoa(i)+"].", field.Type, out); err != nil {
					return err
				}
			}
			continue
		}
		if err := p.flattenFields(prefix+field.Name+".", field.Type, out); err != nil {
			return err
		}
	}
	return nil
}

func (p *parser) parseData(data []byte) (uint64, error) {
	if len(data) < 2 {
		return 0, fmt.Errorf("data message too short")
	}
	msgID := binary.LittleEndian.Uint16(data[:2])
	sub, ok := p.subscriptions[msgID]
	if !ok {
		if _, filtered := p.filteredIDs[msgID]; !filtered {
			p.missingIDs[msgID] = struct{}{}
			return 0, fmt.Errorf("no subscription for message id %d", msgID)
		}
		return 0, nil
	}

	payload := data[2:]
	if len(payload) < sub.minDataSize {
		return 0, fmt.Errorf("data message for %s too short: have %d need %d", sub.messageName, len(payload), sub.minDataSize)
	}
	if len(payload) > sub.minDataSize {
		payload = payload[:sub.minDataSize]
	}

	row := Row{Values: make([]Value, len(sub.fields))}
	for i, field := range sub.fields {
		value, err := decodePrimitive(field.Type, payload[field.Offset:field.Offset+field.Size])
		if err != nil {
			return 0, err
		}
		row.Values[i] = value
		if i == sub.timestampIndex {
			switch ts := value.Any.(type) {
			case uint64:
				row.Timestamp = ts
			case uint32:
				row.Timestamp = uint64(ts)
			case int64:
				row.Timestamp = uint64(ts)
			case int32:
				row.Timestamp = uint64(ts)
			}
		}
	}
	sub.rows = append(sub.rows, row)
	return row.Timestamp, nil
}

func (p *parser) flushDatasets() {
	for _, sub := range p.subscriptions {
		if len(sub.rows) == 0 {
			continue
		}
		p.log.Data = append(p.log.Data, Dataset{
			Name:           sub.messageName,
			MultiID:        sub.multiID,
			MsgID:          sub.msgID,
			Fields:         append([]FlatField(nil), sub.fields...),
			TimestampIndex: sub.timestampIndex,
			Rows:           append([]Row(nil), sub.rows...),
		})
	}
}

func (p *parser) acceptsMessage(name string) bool {
	if p.opts.messageFilter == nil {
		return true
	}
	_, ok := p.opts.messageFilter[name]
	return ok
}

func packetLooksCorrupt(header messageHeader) bool {
	return header.typ == 0 || header.size == 0 || header.size > 10000
}

type parsedInfo struct {
	key       string
	value     Value
	continued bool
}

func parseInfo(data []byte, multiple bool) (parsedInfo, error) {
	info := parsedInfo{}
	if multiple {
		if len(data) < 2 {
			return info, fmt.Errorf("info multiple too short")
		}
		info.continued = data[0] != 0
		data = data[1:]
	}
	if len(data) < 1 {
		return info, fmt.Errorf("info too short")
	}
	keyLen := int(data[0])
	if len(data) < 1+keyLen {
		return info, fmt.Errorf("info key too short")
	}
	typeKey := string(data[1 : 1+keyLen])
	parts := strings.SplitN(typeKey, " ", 2)
	if len(parts) != 2 {
		return info, fmt.Errorf("invalid info key %q", typeKey)
	}
	value, err := decodeInfoValue(parts[0], data[1+keyLen:])
	if err != nil {
		return info, err
	}
	info.key = parts[1]
	info.value = value
	return info, nil
}

func decodeInfoValue(typeName string, data []byte) (Value, error) {
	if strings.HasPrefix(typeName, "char[") {
		return Value{Type: typeName, Any: trimCString(string(data))}, nil
	}
	if strings.Contains(typeName, "[") {
		raw := append([]byte(nil), data...)
		return Value{Type: typeName, Raw: raw}, nil
	}
	return decodePrimitive(typeName, data)
}

func parseFormat(data []byte) (MessageFormat, error) {
	text := string(data)
	name, rest, ok := strings.Cut(text, ":")
	if !ok || name == "" {
		return MessageFormat{}, fmt.Errorf("invalid format message %q", text)
	}
	format := MessageFormat{Name: name}
	for _, raw := range strings.Split(rest, ";") {
		raw = strings.TrimSpace(raw)
		if raw == "" {
			continue
		}
		field, err := parseField(raw)
		if err != nil {
			return MessageFormat{}, err
		}
		format.Fields = append(format.Fields, field)
	}
	return format, nil
}

func parseField(raw string) (Field, error) {
	parts := strings.Fields(raw)
	if len(parts) != 2 {
		return Field{}, fmt.Errorf("invalid field %q", raw)
	}
	field := Field{Name: parts[1], Type: parts[0]}
	if idx := strings.IndexByte(field.Type, '['); idx >= 0 {
		end := strings.IndexByte(field.Type[idx:], ']')
		if end < 0 {
			return Field{}, fmt.Errorf("invalid array field %q", raw)
		}
		size, err := strconv.Atoi(field.Type[idx+1 : idx+end])
		if err != nil {
			return Field{}, fmt.Errorf("invalid array size in %q: %w", raw, err)
		}
		field.ArraySize = size
		field.Type = field.Type[:idx]
	}
	return field, nil
}

func parseLogging(data []byte) (LoggingMessage, error) {
	if len(data) < 9 {
		return LoggingMessage{}, fmt.Errorf("logging message too short")
	}
	return LoggingMessage{
		LogLevel:  data[0],
		Timestamp: binary.LittleEndian.Uint64(data[1:9]),
		Message:   string(data[9:]),
	}, nil
}

func parseTaggedLogging(data []byte) (TaggedLoggingMessage, error) {
	if len(data) < 11 {
		return TaggedLoggingMessage{}, fmt.Errorf("tagged logging message too short")
	}
	return TaggedLoggingMessage{
		LogLevel:  data[0],
		Tag:       binary.LittleEndian.Uint16(data[1:3]),
		Timestamp: binary.LittleEndian.Uint64(data[3:11]),
		Message:   string(data[11:]),
	}, nil
}