MARTe-Integrated-Components/Common/Client/go/wshub/sources.go

package wshub

import (
	"fmt"
	"io"
	"log"
	"net"
	"os"
	"sort"
	"strconv"
	"strings"
	"sync"
	"sync/atomic"
	"time"

	"marte2/common/udpsprotocol"
)

// ─── Source configuration ─────────────────────────────────────────────────────

// SourceConfig is the serialisable description of one data source (for file save/load).
type SourceConfig struct {
	Label          string `json:"label"`
	Addr           string `json:"addr"`
	MulticastGroup string `json:"multicastGroup,omitempty"`
	DataPort       int    `json:"dataPort,omitempty"`
}

// managedSource is the SourceManager's view of one running source.
type managedSource struct {
	id             string
	label          string
	addr           string
	multicastGroup string
	dataPort       int
	client         *UDPClient
}

// SourceManager owns the lifecycle of all active data sources.
type SourceManager struct {
	mu       sync.RWMutex
	sources  map[string]*managedSource
	hub      *Hub
	filePath string
	nextID   atomic.Int32
}

// NewSourceManager creates a SourceManager bound to the given hub.
func NewSourceManager(hub *Hub, filePath string) *SourceManager {
	return &SourceManager{
		sources:  make(map[string]*managedSource),
		hub:      hub,
		filePath: filePath,
	}
}

func (sm *SourceManager) genID() string {
	return fmt.Sprintf("s%d", sm.nextID.Add(1))
}

// Add creates a new source and starts connecting.
func (sm *SourceManager) Add(label, addr, multicastGroup string, dataPort int) string {
	if label == "" {
		label = addr
	}
	id := sm.genID()
	c := NewUDPClient(addr, id, sm.hub, multicastGroup, dataPort)
	ms := &managedSource{
		id: id, label: label, addr: addr,
		multicastGroup: multicastGroup, dataPort: dataPort,
		client: c,
	}

	sm.mu.Lock()
	sm.sources[id] = ms
	sm.mu.Unlock()

	sm.hub.AddSource(id, label, addr)
	go c.Run()
	return id
}

// Remove stops the source and removes it from the hub.
func (sm *SourceManager) Remove(id string) {
	sm.mu.Lock()
	ms, ok := sm.sources[id]
	if ok {
		delete(sm.sources, id)
	}
	sm.mu.Unlock()

	if ok {
		ms.client.Stop()
		sm.hub.RemoveSource(id)
	}
}

// Path returns the configured config-file path ("" when none).
func (sm *SourceManager) Path() string {
	sm.mu.RLock()
	defer sm.mu.RUnlock()
	return sm.filePath
}

// snapshotSources returns the current sources sorted by label, so the written
// file is byte-stable across runs (the map iteration order is not).
func (sm *SourceManager) snapshotSources() []SourceConfig {
	sm.mu.RLock()
	cfgs := make([]SourceConfig, 0, len(sm.sources))
	for _, ms := range sm.sources {
		cfgs = append(cfgs, SourceConfig{
			Label:          ms.label,
			Addr:           ms.addr,
			MulticastGroup: ms.multicastGroup,
			DataPort:       ms.dataPort,
		})
	}
	sm.mu.RUnlock()
	sort.Slice(cfgs, func(i, j int) bool {
		if cfgs[i].Label != cfgs[j].Label {
			return cfgs[i].Label < cfgs[j].Label
		}
		return cfgs[i].Addr < cfgs[j].Addr
	})
	return cfgs
}

// Save writes the current source list and calibration table to filePath as one
// flat JSON array.
func (sm *SourceManager) Save() error {
	path := sm.Path()
	if path == "" {
		return fmt.Errorf("no sources-file configured")
	}
	data, err := encodeConfigFile(sm.snapshotSources(), sm.hub.cal.List())
	if err != nil {
		return err
	}
	return os.WriteFile(path, data, 0644)
}

// Load reads the config file at path, replaces the calibration table with its
// contents and starts every source it lists.
func (sm *SourceManager) Load(path string) error {
	data, err := os.ReadFile(path)
	if err != nil {
		return err
	}
	srcs, cals, err := parseConfigFile(data)
	if err != nil {
		return err
	}
	sm.mu.Lock()
	sm.filePath = path
	sm.mu.Unlock()

	sm.hub.cal.Replace(cals)
	for _, cfg := range srcs {
		sm.Add(cfg.Label, cfg.Addr, cfg.MulticastGroup, cfg.DataPort)
	}
	return nil
}

// Reload re-reads the config file.  The calibration table is replaced wholesale
// and sources listed in the file that are not already running are started; no
// live source is ever stopped, restarted or reconnected, because a reload must
// not interrupt streaming.  The asymmetry is deliberate: calibration is cheap
// to reapply, a source is a live UDP session.
func (sm *SourceManager) Reload() error {
	path := sm.Path()
	if path == "" {
		return fmt.Errorf("no sources-file configured")
	}
	data, err := os.ReadFile(path)
	if err != nil {
		return err
	}
	srcs, cals, err := parseConfigFile(data)
	if err != nil {
		return err
	}
	sm.hub.cal.Replace(cals)

	sm.mu.RLock()
	live := make(map[string]bool, len(sm.sources))
	for _, ms := range sm.sources {
		live[ms.label+"\x00"+ms.addr] = true
	}
	sm.mu.RUnlock()

	for _, cfg := range srcs {
		label := cfg.Label
		if label == "" {
			label = cfg.Addr // Add() applies the same default
		}
		if live[label+"\x00"+cfg.Addr] {
			continue
		}
		sm.Add(cfg.Label, cfg.Addr, cfg.MulticastGroup, cfg.DataPort)
	}
	return nil
}

// ParseSourceArg parses "label@host:port" or "host:port".
func ParseSourceArg(s string) (label, addr string) {
	label, addr, _, _ = ParseSourceArgFull(s)
	return
}

// ParseSourceArgFull parses "[label@]host:port[/multicastGroup:dataPort]".
func ParseSourceArgFull(s string) (label, addr, multicastGroup string, dataPort int) {
	s = strings.TrimSpace(s)
	rest := s
	if idx := strings.Index(s, "@"); idx >= 0 {
		label = strings.TrimSpace(s[:idx])
		rest = strings.TrimSpace(s[idx+1:])
	}
	if idx := strings.Index(rest, "/"); idx >= 0 {
		addr = strings.TrimSpace(rest[:idx])
		mcastPart := strings.TrimSpace(rest[idx+1:])
		if lastColon := strings.LastIndex(mcastPart, ":"); lastColon >= 0 {
			multicastGroup = strings.TrimSpace(mcastPart[:lastColon])
			dataPort, _ = strconv.Atoi(strings.TrimSpace(mcastPart[lastColon+1:]))
		} else {
			multicastGroup = mcastPart
		}
	} else {
		addr = rest
	}
	return
}

// ─── UDPClient ────────────────────────────────────────────────────────────────

const (
	silenceTimeout = 5 * time.Second
	reconnectDelay = 2 * time.Second
	readBufSize    = 65536
	udpRcvBufSize  = 8 * 1024 * 1024
	// keepAliveInterval is the unicast keepalive period. The UDPStreamer
	// server evicts silent unicast clients after its ClientTimeout (default
	// 30 s); an ACK from the same socket refreshes its last-seen without
	// triggering a CONFIG resend (a CONNECT would).
	keepAliveInterval = 15 * time.Second
)

// UDPClient manages the connection to one MARTe2 streamer source.
type UDPClient struct {
	serverAddr        string
	sourceID          string
	hub               *Hub
	multicastGroup    string
	dataPort          int
	keepAliveInterval time.Duration
	stopCh            chan struct{}
}

// NewUDPClient creates a UDPClient bound to a specific source ID.
func NewUDPClient(serverAddr, sourceID string, hub *Hub, multicastGroup string, dataPort int) *UDPClient {
	return &UDPClient{
		serverAddr:        serverAddr,
		sourceID:          sourceID,
		hub:               hub,
		multicastGroup:    multicastGroup,
		dataPort:          dataPort,
		keepAliveInterval: keepAliveInterval,
		stopCh:            make(chan struct{}),
	}
}

// Stop asks the client to disconnect and exit.
func (u *UDPClient) Stop() {
	close(u.stopCh)
}

// Run is the main loop; it reconnects automatically if the server goes silent.
func (u *UDPClient) Run() {
	for {
		select {
		case <-u.stopCh:
			return
		default:
		}

		u.hub.SetSourceState(u.sourceID, "connecting")
		log.Printf("[%s] connecting to %s", u.sourceID, u.serverAddr)

		var err error
		if u.multicastGroup != "" {
			err = u.runMulticastSession()
		} else {
			err = u.runSession()
		}
		if err != nil {
			log.Printf("[%s] session ended: %v", u.sourceID, err)
		}
		u.hub.SetSourceState(u.sourceID, "disconnected")

		select {
		case <-u.stopCh:
			return
		case <-time.After(reconnectDelay):
		}
	}
}

// runSession opens a UDP socket, sends CONNECT, reads data until silent or error.
func (u *UDPClient) runSession() error {
	conn, err := net.ListenUDP("udp4", &net.UDPAddr{})
	if err != nil {
		return err
	}
	defer conn.Close()

	if err := conn.SetReadBuffer(udpRcvBufSize); err != nil {
		log.Printf("[%s] udp: SetReadBuffer: %v", u.sourceID, err)
	}

	serverAddr, err := net.ResolveUDPAddr("udp4", u.serverAddr)
	if err != nil {
		return err
	}

	if _, err := conn.WriteToUDP(udpsprotocol.BuildConnectPacket(), serverAddr); err != nil {
		return err
	}
	log.Printf("[%s] udp: sent CONNECT", u.sourceID)
	lastData := time.Now()
	lastKeepAlive := time.Now()
	// sendKeepAliveIfDue sends an ACK if the keepalive interval has elapsed.
	// ACK refreshes the server's last-seen without re-sending CONFIG (which a
	// repeated CONNECT would trigger).
	sendKeepAliveIfDue := func() error {
		if u.keepAliveInterval > 0 && time.Since(lastKeepAlive) >= u.keepAliveInterval {
			if _, err := conn.WriteToUDP(udpsprotocol.BuildAckPacket(), serverAddr); err != nil {
				return err
			}
			lastKeepAlive = time.Now()
		}
		return nil
	}

	reassembler := udpsprotocol.NewReassembler(2 * time.Second)
	// Per-session: the producer's counter restarts independently of ours, so
	// the gate must not carry a counter over from the previous connection.
	var gate udpsprotocol.SequenceGate
	buf := make([]byte, readBufSize)
	var currentSigs []udpsprotocol.SignalInfo
	var currentPublishMode uint8

	for {
		// Wake up at least every keepalive interval so ACKs are sent even
		// when the server is idle; the read deadline also doubles as the
		// silence detector (no data for silenceTimeout = server gone).
		wakeup := silenceTimeout
		if u.keepAliveInterval > 0 && u.keepAliveInterval < wakeup {
			wakeup = u.keepAliveInterval
		}
		conn.SetReadDeadline(time.Now().Add(wakeup))

		n, _, err := conn.ReadFromUDP(buf)
		arrivalTime := time.Now()
		if err != nil {
			if ne, ok := err.(net.Error); ok && ne.Timeout() {
				if time.Since(lastData) >= silenceTimeout {
					// True silence: stream is dead — Run() reconnects.
					conn.WriteToUDP(udpsprotocol.BuildDisconnectPacket(), serverAddr)
					return err
				}
				// Short wakeup: keepalive if due, then keep waiting.
				if kaErr := sendKeepAliveIfDue(); kaErr != nil {
					return kaErr
				}
				continue
			}
			conn.WriteToUDP(udpsprotocol.BuildDisconnectPacket(), serverAddr)
			return err
		}
		lastData = arrivalTime

		if n < udpsprotocol.HeaderSize {
			log.Printf("[%s] udp: short datagram (%d bytes), skipping", u.sourceID, n)
			continue
		}

		hdr, err := udpsprotocol.ParseHeader(buf[:n])
		if err != nil {
			log.Printf("[%s] udp: parse header: %v", u.sourceID, err)
			continue
		}

		payload := make([]byte, n-udpsprotocol.HeaderSize)
		copy(payload, buf[udpsprotocol.HeaderSize:n])

		complete, ok := reassembler.AddFragment(hdr, payload)
		if hdr.Type == udpsprotocol.PktData {
			u.hub.RecordDataFragment(u.sourceID, hdr.Counter, n, arrivalTime.UnixNano(), ok)
		}
		if !ok {
			continue
		}

		switch hdr.Type {
		case udpsprotocol.PktConfig:
			sigs, pm, err := udpsprotocol.ParseConfig(complete)
			if err != nil {
				log.Printf("[%s] udp: parse config: %v", u.sourceID, err)
				continue
			}
			currentSigs = sigs
			currentPublishMode = pm
			log.Printf("[%s] udp: received CONFIG (%d signals, publishMode=%d)", u.sourceID, len(sigs), pm)
			u.hub.SetSourceState(u.sourceID, "connected")
			u.hub.UpdateConfigForSource(u.sourceID, sigs)

		case udpsprotocol.PktData:
			if len(currentSigs) == 0 {
				continue
			}
			fresh, lost := gate.Accept(hdr.Counter)
			if !fresh {
				continue
			}
			samples, err := udpsprotocol.ParseData(complete, currentSigs, currentPublishMode, arrivalTime)
			if err != nil {
				log.Printf("[%s] udp: parse data: %v", u.sourceID, err)
				continue
			}
			// The gap precedes the packet, so it belongs to its first slot only;
			// the slots after it are consecutive cycles of the same batch.
			if len(samples) > 0 {
				samples[0].Lost = lost
			}
			for _, s := range samples {
				u.hub.PushDataForSource(u.sourceID, s)
			}

		case udpsprotocol.PktACK:
			log.Printf("[%s] udp: received ACK (counter=%d)", u.sourceID, hdr.Counter)

		case udpsprotocol.PktDisconnect:
			log.Printf("[%s] udp: server sent DISCONNECT", u.sourceID)
			return nil

		default:
			log.Printf("[%s] udp: unknown packet type %d", u.sourceID, hdr.Type)
		}

		select {
		case <-u.stopCh:
			conn.WriteToUDP(udpsprotocol.BuildDisconnectPacket(), serverAddr)
			return nil
		default:
		}

		if kaErr := sendKeepAliveIfDue(); kaErr != nil {
			return kaErr
		}
	}
}

// interfaceForIP returns the interface that owns the given local address, or
// nil if no interface matches (in which case callers fall back to letting the
// kernel choose).
func interfaceForIP(ip net.IP) *net.Interface {
	if ip == nil || ip.IsUnspecified() {
		return nil
	}
	ifaces, err := net.Interfaces()
	if err != nil {
		return nil
	}
	for i := range ifaces {
		addrs, err := ifaces[i].Addrs()
		if err != nil {
			continue
		}
		for _, a := range addrs {
			var aIP net.IP
			switch v := a.(type) {
			case *net.IPNet:
				aIP = v.IP
			case *net.IPAddr:
				aIP = v.IP
			}
			if aIP != nil && aIP.Equal(ip) {
				return &ifaces[i]
			}
		}
	}
	return nil
}

// interfaceForConn returns the interface a connection's local endpoint sits on.
func interfaceForConn(c net.Conn) *net.Interface {
	if c == nil {
		return nil
	}
	switch a := c.LocalAddr().(type) {
	case *net.TCPAddr:
		return interfaceForIP(a.IP)
	case *net.UDPAddr:
		return interfaceForIP(a.IP)
	}
	return nil
}

// runMulticastSession handles the multicast mode session.
func (u *UDPClient) runMulticastSession() error {
	tcpAddr, err := net.ResolveTCPAddr("tcp4", u.serverAddr)
	if err != nil {
		return err
	}

	tcpConn, err := net.DialTCP("tcp4", nil, tcpAddr)
	if err != nil {
		return err
	}
	defer tcpConn.Close()

	if _, err := tcpConn.Write(udpsprotocol.BuildConnectPacket()); err != nil {
		return err
	}
	log.Printf("[%s] tcp: sent CONNECT to %s", u.sourceID, u.serverAddr)

	hdrBuf := make([]byte, udpsprotocol.HeaderSize)
	if _, err := io.ReadFull(tcpConn, hdrBuf); err != nil {
		return err
	}
	cfgHdr, err := udpsprotocol.ParseHeader(hdrBuf)
	if err != nil {
		return err
	}
	if cfgHdr.Type != udpsprotocol.PktConfig {
		return net.ErrClosed
	}
	cfgPayload := make([]byte, cfgHdr.PayloadBytes)
	if cfgHdr.PayloadBytes > 0 {
		if _, err := io.ReadFull(tcpConn, cfgPayload); err != nil {
			return err
		}
	}
	currentSigs, currentPublishMode, err := udpsprotocol.ParseConfig(cfgPayload)
	if err != nil {
		return err
	}
	log.Printf("[%s] tcp: received CONFIG (%d signals, publishMode=%d)", u.sourceID, len(currentSigs), currentPublishMode)
	u.hub.SetSourceState(u.sourceID, "connected")
	u.hub.UpdateConfigForSource(u.sourceID, currentSigs)

	mcastPort := u.dataPort
	if mcastPort == 0 {
		mcastPort = tcpAddr.Port + 1
	}

	mcastIP := net.ParseIP(u.multicastGroup)
	if mcastIP == nil {
		return &net.AddrError{Err: "invalid multicast group IP", Addr: u.multicastGroup}
	}
	mcastAddr := &net.UDPAddr{IP: mcastIP, Port: mcastPort}
	// Join on the interface that reaches the control connection. The UDPStreamer
	// pins its multicast sends to its configured Interface (IP_MULTICAST_IF), so
	// a join with a nil interface — which leaves imr_interface at INADDR_ANY and
	// lets the kernel pick the default-route interface — silently receives
	// nothing whenever that is not the sending interface. The local address of
	// the control connection is the interface the server is reachable on, which
	// is the sending interface in every single-homed and same-host deployment.
	ifi := interfaceForConn(tcpConn)
	mcastConn, err := net.ListenMulticastUDP("udp4", ifi, mcastAddr)
	if err != nil {
		return err
	}
	defer mcastConn.Close()
	if err := mcastConn.SetReadBuffer(udpRcvBufSize); err != nil {
		log.Printf("[%s] multicast SetReadBuffer: %v", u.sourceID, err)
	}
	ifName := "default"
	if ifi != nil {
		ifName = ifi.Name
	}
	log.Printf("[%s] joined multicast %s:%s on interface %s",
		u.sourceID, u.multicastGroup, strconv.Itoa(mcastPort), ifName)

	tcpDone := make(chan error, 1)
	go func() {
		buf := make([]byte, udpsprotocol.HeaderSize+64)
		for {
			n, readErr := tcpConn.Read(buf)
			if readErr != nil {
				tcpDone <- readErr
				return
			}
			if n >= udpsprotocol.HeaderSize {
				hdr, parseErr := udpsprotocol.ParseHeader(buf[:n])
				if parseErr == nil && hdr.Type == udpsprotocol.PktDisconnect {
					tcpDone <- nil
					return
				}
			}
		}
	}()

	reassembler := udpsprotocol.NewReassembler(2 * time.Second)
	// Per-session, as in runSession(): a counter from the previous connection
	// would reject the whole new stream.
	var gate udpsprotocol.SequenceGate
	buf := make([]byte, readBufSize)

	for {
		mcastConn.SetReadDeadline(time.Now().Add(silenceTimeout))
		n, _, readErr := mcastConn.ReadFromUDP(buf)
		arrivalTime := time.Now()
		if readErr != nil {
			select {
			case <-tcpDone:
				return nil
			default:
			}
			tcpConn.Write(udpsprotocol.BuildDisconnectPacket())
			return readErr
		}

		if n < udpsprotocol.HeaderSize {
			continue
		}
		hdr, parseErr := udpsprotocol.ParseHeader(buf[:n])
		if parseErr != nil {
			log.Printf("[%s] multicast: parse header: %v", u.sourceID, parseErr)
			continue
		}

		payload := make([]byte, n-udpsprotocol.HeaderSize)
		copy(payload, buf[udpsprotocol.HeaderSize:n])

		complete, ok := reassembler.AddFragment(hdr, payload)
		if hdr.Type == udpsprotocol.PktData {
			u.hub.RecordDataFragment(u.sourceID, hdr.Counter, n, arrivalTime.UnixNano(), ok)
		}
		if !ok {
			continue
		}

		if hdr.Type == udpsprotocol.PktData {
			if len(currentSigs) == 0 {
				continue
			}
			fresh, lost := gate.Accept(hdr.Counter)
			if !fresh {
				continue
			}
			samples, parseErr := udpsprotocol.ParseData(complete, currentSigs, currentPublishMode, arrivalTime)
			if parseErr != nil {
				log.Printf("[%s] multicast: parse data: %v", u.sourceID, parseErr)
				continue
			}
			if len(samples) > 0 {
				samples[0].Lost = lost
			}
			for _, s := range samples {
				u.hub.PushDataForSource(u.sourceID, s)
			}
		}

		select {
		case <-u.stopCh:
			tcpConn.Write(udpsprotocol.BuildDisconnectPacket())
			return nil
		case tcpErr := <-tcpDone:
			log.Printf("[%s] tcp control closed: %v", u.sourceID, tcpErr)
			return nil
		default:
		}
	}
}