silver_pose/v2/internal/fall/state.go

package fall

import "fmt"

type stateRecord struct {
	state State

	suspectStartedAt     float64
	hasSuspectStartedAt  bool
	confirmedAt          float64
	hasConfirmedAt       bool
	recoveryStartedAt    float64
	hasRecoveryStartedAt bool
	lastUpdatedAt        float64
	hasLastUpdatedAt     bool
}

type stateMachine struct {
	confirmWindowSeconds  float64
	recoveryWindowSeconds float64
	cooldownSeconds       float64
	configVersion         string
	sessionID             string
	records               map[string]*stateRecord
	nextEventNumber       int
}

func newStateMachine(config EngineConfig) (*stateMachine, error) {
	if config.ConfirmWindowSeconds < 1 || config.ConfirmWindowSeconds > 3 {
		return nil, fmt.Errorf("confirm window seconds must be between 1 and 3")
	}
	if config.RecoveryWindowSeconds <= 0 {
		return nil, fmt.Errorf("recovery window seconds must be positive")
	}
	if config.CooldownSeconds < 0 {
		return nil, fmt.Errorf("cooldown seconds must be non-negative")
	}
	if config.ConfigVersion == "" {
		return nil, fmt.Errorf("config version must be non-empty")
	}
	return &stateMachine{
		confirmWindowSeconds: config.ConfirmWindowSeconds, recoveryWindowSeconds: config.RecoveryWindowSeconds,
		cooldownSeconds: config.CooldownSeconds, configVersion: config.ConfigVersion, sessionID: config.SessionID,
		records: make(map[string]*stateRecord), nextEventNumber: 1,
	}, nil
}

func (machine *stateMachine) stateOf(trackID string) State {
	if record, found := machine.records[trackID]; found {
		return record.state
	}
	return Normal
}

func (machine *stateMachine) update(trackID string, evidence Evidence, now float64) ([]Event, error) {
	if trackID == "" {
		return nil, fmt.Errorf("track ID must be non-empty")
	}
	record, found := machine.records[trackID]
	if !found {
		record = &stateRecord{state: Normal}
		machine.records[trackID] = record
	}
	if record.hasLastUpdatedAt && now < record.lastUpdatedAt {
		return nil, fmt.Errorf("timestamps must be monotonic per track")
	}
	record.lastUpdatedAt, record.hasLastUpdatedAt = now, true

	if !evidence.Accepted {
		machine.rejectEvidence(record)
		return nil, nil
	}
	switch record.state {
	case Normal:
		if evidence.IsFallCandidate {
			record.state, record.suspectStartedAt, record.hasSuspectStartedAt = Suspect, now, true
		}
		return nil, nil
	case Suspect:
		if !evidence.IsFallCandidate {
			machine.setNormal(record)
			return nil, nil
		}
		if now-record.suspectStartedAt >= machine.confirmWindowSeconds {
			record.state, record.confirmedAt, record.hasConfirmedAt = Confirmed, now, true
			event := machine.newEvent(trackID, record.suspectStartedAt, now)
			record.hasSuspectStartedAt = false
			return []Event{event}, nil
		}
		return nil, nil
	case Confirmed:
		if evidence.IsRecoveryCandidate && now-record.confirmedAt >= machine.cooldownSeconds {
			record.state, record.recoveryStartedAt, record.hasRecoveryStartedAt = Recovering, now, true
		}
		return nil, nil
	case Recovering:
		if evidence.IsFallCandidate {
			record.state, record.suspectStartedAt, record.hasSuspectStartedAt = Suspect, now, true
			record.hasRecoveryStartedAt = false
			return nil, nil
		}
		if !evidence.IsRecoveryCandidate {
			record.state, record.hasRecoveryStartedAt = Confirmed, false
			return nil, nil
		}
		if now-record.recoveryStartedAt >= machine.recoveryWindowSeconds {
			machine.setNormal(record)
		}
		return nil, nil
	default:
		return nil, fmt.Errorf("unknown fall state")
	}
}

func (machine *stateMachine) rejectEvidence(record *stateRecord) {
	if record.state == Suspect {
		machine.setNormal(record)
	} else if record.state == Recovering {
		record.state, record.hasRecoveryStartedAt = Confirmed, false
	}
}

func (machine *stateMachine) setNormal(record *stateRecord) {
	record.state = Normal
	record.hasSuspectStartedAt = false
	record.hasConfirmedAt = false
	record.hasRecoveryStartedAt = false
}

func (machine *stateMachine) newEvent(trackID string, suspectedAt, confirmedAt float64) Event {
	prefix := "FALL-"
	if machine.sessionID != "" {
		prefix += machine.sessionID + "-"
	}
	event := Event{
		EventID: fmt.Sprintf("%s%06d", prefix, machine.nextEventNumber), TrackID: trackID,
		ConfigVersion: machine.configVersion, SuspectedAtMonotonic: suspectedAt,
		ConfirmedAtMonotonic: confirmedAt, LatencySeconds: confirmedAt - suspectedAt, State: Confirmed,
	}
	machine.nextEventNumber++
	return event
}