diff --git a/mothership/cmd/sim/generator.go b/mothership/cmd/sim/generator.go new file mode 100644 index 0000000..7fce59b --- /dev/null +++ b/mothership/cmd/sim/generator.go @@ -0,0 +1,278 @@ +// Package main provides CSI frame generation for the simulator. +package main + +import ( + "encoding/binary" + "fmt" + "math" + + "github.com/spaxel/mothership/internal/simulator" +) + +const ( + // CSI frame header size (24 bytes per Phase 1 protocol) + HeaderSize = 24 + + // Magic number for CSI frame identification + CSIMagic = 0xABCDEF01 + + // Protocol version + CSIVersion = 1 + + // Default number of subcarriers (HT20) + DefaultSubcarriers = 52 + + // Maximum subcarriers (HT20 64 total) + MaxSubcarriers = 64 + + // WiFi channels + DefaultChannel = 6 // 2.4 GHz channel 6 + Channel5GHzStart = 36 // 5 GHz channel 36 +) + +// CSIFrameGenerator generates synthetic CSI binary frames +type CSIFrameGenerator struct { + nodeMAC []byte + nodePosition simulator.Point + peerMAC []byte + peerPosition simulator.Point + channel uint8 + frameIndex uint64 + physics *simulator.PhysicsModel +} + +// NewCSIFrameGenerator creates a new CSI frame generator +func NewCSIFrameGenerator(nodeMAC string, nodePos simulator.Point, physics *simulator.PhysicsModel) *CSIFrameGenerator { + macBytes := macStringToBytes(nodeMAC) + + return &CSIFrameGenerator{ + nodeMAC: macBytes[:], + nodePosition: nodePos, + peerMAC: []byte{0x11, 0x22, 0x33, 0x44, 0x55, 0x00}, // Default peer + peerPosition: simulator.Point{X: 0, Y: 0, Z: 1.7}, + channel: DefaultChannel, + frameIndex: 0, + physics: physics, + } +} + +// SetPeer sets the peer (transmitter) MAC and position +func (g *CSIFrameGenerator) SetPeer(peerMAC string, pos simulator.Point) { + macBytes := macStringToBytes(peerMAC) + copy(g.peerMAC, macBytes[:]) + g.peerPosition = pos +} + +// SetChannel sets the WiFi channel +func (g *CSIFrameGenerator) SetChannel(channel uint8) { + g.channel = channel +} + +// SetFrameIndex sets the current frame index +func (g *CSIFrameGenerator) SetFrameIndex(index uint64) { + g.frameIndex = index +} + +// GenerateFrame generates a synthetic CSI frame for a walker at the given position +// Returns the complete binary frame ready to send over WebSocket +func (g *CSIFrameGenerator) GenerateFrame(walkerPos simulator.Point) []byte { + nSub := DefaultSubcarriers + + // Calculate frame size + frameSize := HeaderSize + nSub*2 + buf := make([]byte, frameSize) + + // Calculate distance to walker for RSSI + distance := g.nodePosition.Distance(walkerPos) + rssi := g.physics.ComputeRSSI(distance) + + // Calculate Fresnel modulation + fresnelMod := simulator.ComputeFresnelModulation(g.nodePosition, g.peerPosition, walkerPos) + + // Write header + g.writeHeader(buf, nSub, rssi, walkerPos) + + // Generate subcarrier CSI payload + g.writePayload(buf[HeaderSize:], walkerPos, fresnelMod, nSub) + + // Increment frame index + g.frameIndex++ + + return buf +} + +// writeHeader writes the 24-byte CSI frame header matching the Phase 1 protocol +func (g *CSIFrameGenerator) writeHeader(buf []byte, nSub int, rssi int8, walkerPos simulator.Point) { + // According to the task, header is 24 bytes: + // Bytes 0-5: Node MAC (6 bytes) + // Bytes 6-11: Peer MAC (6 bytes) + // Bytes 12-19: Timestamp_us (8 bytes, uint64) + // Byte 20: RSSI (1 byte, signed) + // Byte 21: Noise floor (1 byte, signed) + // Byte 22: Channel (1 byte) + // Byte 23: Num subcarriers (1 byte) + + // Bytes 0-5: Node MAC + copy(buf[0:6], g.nodeMAC) + + // Bytes 6-11: Peer MAC + copy(buf[6:12], g.peerMAC) + + // Bytes 12-19: Timestamp microseconds (little-endian uint64) + // Default to 20 Hz for timestamp calculation + timestampUS := g.frameIndex * (1_000_000 / 20) + binary.LittleEndian.PutUint64(buf[12:20], timestampUS) + + // Byte 20: RSSI (signed int8) + buf[20] = byte(rssi) + + // Byte 21: Noise floor (signed int8) - typical -95 dBm + buf[21] = 0xA1 // -95 as signed int8 bit pattern + + // Byte 22: Channel + buf[22] = g.channel + + // Byte 23: Number of subcarriers + buf[23] = byte(nSub) +} + +// writePayload writes the I/Q payload for each subcarrier +func (g *CSIFrameGenerator) writePayload(payload []byte, walkerPos simulator.Point, fresnelMod float64, nSub int) { + // Get base amplitude from deltaRMS + deltaRMS := g.physics.DeltaRMS(g.peerPosition, g.nodePosition, walkerPos) + amplitude := deltaRMS * 500.0 // Scale to reasonable I/Q range + + // Apply Fresnel modulation + modulation := 1.0 + fresnelMod*2.0 // Enhance signal when in zone 1 + scaledAmplitude := amplitude * modulation + + // Generate I/Q pairs for each subcarrier + for k := 0; k < nSub; k++ { + // Compute phase at this subcarrier using physics model + phase := g.physics.PhaseAtSubcarrier( + g.peerPosition, + g.nodePosition, + walkerPos, + k, + int(g.frameIndex), + ) + + // Add subcarrier-dependent amplitude variation (frequency-selective fading) + freqFading := 0.8 + 0.4*math.Sin(2*math.Pi*float64(k)/16.0) + subAmplitude := scaledAmplitude * freqFading + + // Generate I/Q pair with noise + i, q := g.physics.GenerateIQPair(subAmplitude, phase) + + payload[k*2] = byte(i) + payload[k*2+1] = byte(q) + } +} + +// macStringToBytes converts MAC address string to byte slice +func macStringToBytes(mac string) [6]byte { + var b [6]byte + fmt.Sscanf(mac, "%02X:%02X:%02X:%02X:%02X:%02X", + &b[0], &b[1], &b[2], &b[3], &b[4], &b[5]) + return b +} + +// ValidateFrame validates a generated CSI frame +func ValidateFrame(frame []byte) error { + // Check minimum frame length + if len(frame) < HeaderSize { + return fmt.Errorf("frame too short: %d bytes (minimum %d)", len(frame), HeaderSize) + } + + // Get n_sub from byte 23 + nSub := int(frame[23]) + + // Validate payload length matches + expectedLen := HeaderSize + nSub*2 + if len(frame) != expectedLen { + return fmt.Errorf("payload length mismatch: got %d, expected %d (n_sub=%d)", + len(frame), expectedLen, nSub) + } + + // Validate n_sub range + if nSub > MaxSubcarriers { + return fmt.Errorf("n_sub too large: %d (max %d)", nSub, MaxSubcarriers) + } + + // Validate channel is valid WiFi channel (1-14 for 2.4 GHz) + channel := frame[22] + if channel < 1 || channel > 14 { + return fmt.Errorf("invalid channel: %d (valid range 1-14)", channel) + } + + return nil +} + +// GetRSSIFromFrame extracts RSSI from a CSI frame +func GetRSSIFromFrame(frame []byte) (int8, error) { + if len(frame) < HeaderSize { + return 0, fmt.Errorf("frame too short") + } + return int8(frame[20]), nil +} + +// GetChannelFromFrame extracts channel from a CSI frame +func GetChannelFromFrame(frame []byte) (uint8, error) { + if len(frame) < HeaderSize { + return 0, fmt.Errorf("frame too short") + } + return frame[22], nil +} + +// GetTimestampFromFrame extracts timestamp from a CSI frame +func GetTimestampFromFrame(frame []byte) (uint64, error) { + if len(frame) < HeaderSize { + return 0, fmt.Errorf("frame too short") + } + return binary.LittleEndian.Uint64(frame[12:20]), nil +} + +// GetSubcarrierCount extracts number of subcarriers from a CSI frame +func GetSubcarrierCount(frame []byte) (int, error) { + if len(frame) < HeaderSize { + return 0, fmt.Errorf("frame too short") + } + return int(frame[23]), nil +} + +// GetIQPair extracts I and Q values for a specific subcarrier +func GetIQPair(frame []byte, subcarrierIndex int) (int8, int8, error) { + if len(frame) < HeaderSize { + return 0, 0, fmt.Errorf("frame too short") + } + + nSub := int(frame[23]) + if subcarrierIndex >= nSub { + return 0, 0, fmt.Errorf("subcarrier index %d out of range (n_sub=%d)", + subcarrierIndex, nSub) + } + + offset := HeaderSize + subcarrierIndex*2 + if offset+2 > len(frame) { + return 0, 0, fmt.Errorf("frame truncated reading subcarrier %d", subcarrierIndex) + } + + i := int8(frame[offset]) + q := int8(frame[offset+1]) + + return i, q, nil +} + +// ComputeExpectedRSSI computes expected RSSI for a given distance +// using the same physics model as the frame generator +func ComputeExpectedRSSI(distance float64, physics *simulator.PhysicsModel) int8 { + return physics.ComputeRSSI(distance) +} + +// DistanceToWalker computes distance from node to walker +func DistanceToWalker(nodePos, walkerPos simulator.Point) float64 { + dx := walkerPos.X - nodePos.X + dy := walkerPos.Y - nodePos.Y + dz := walkerPos.Z - nodePos.Z + return math.Sqrt(dx*dx + dy*dy + dz*dz) +} diff --git a/mothership/cmd/sim/main.go b/mothership/cmd/sim/main.go index e531aab..7c66c5f 100644 --- a/mothership/cmd/sim/main.go +++ b/mothership/cmd/sim/main.go @@ -4,6 +4,7 @@ package main import ( "encoding/binary" + "encoding/csv" "encoding/json" "errors" "flag" @@ -20,6 +21,7 @@ import ( "time" "github.com/gorilla/websocket" + "github.com/spaxel/mothership/internal/simulator" ) // Version is set by the build process via -ldflags @@ -48,7 +50,7 @@ var ( nodes = flag.Int("nodes", 4, "Number of virtual nodes to simulate") walkers = flag.Int("walkers", 1, "Number of walking persons to simulate") rate = flag.Int("rate", 20, "CSI packet rate in Hz") - duration = flag.Duration("duration", 30*time.Second, "Simulation duration") + duration = flag.Duration("duration", 30*time.Second, "Simulation duration (0 = run until Ctrl+C)") enableBLE = flag.Bool("ble", false, "Also send simulated BLE advertisements") seed = flag.Int64("seed", 42, "Random seed for reproducible runs") spaceWidth = flag.Float64("width", 6.0, "Space width in meters") @@ -57,6 +59,13 @@ var ( spaceDims = flag.String("space", "", "Space dimensions as 'WxDxH' (meters), overrides --width/--depth/--height") showFrameRate = flag.Bool("show-frame-rate", true, "Show per-second frame counts to stdout") verbose = flag.Bool("verbose", false, "Enable verbose logging") + + // New flags for verification and advanced simulation + verifyMode = flag.Bool("verify", false, "Verify blob count after simulation (exit code 0=pass, 1=fail)") + noiseSigma = flag.Float64("noise-sigma", 0.005, "Gaussian noise standard deviation for I/Q generation") + wallDefs = flag.String("wall", "", "Add wall as 'x1,y1,x2,y2' (can be repeated)") + outputCSV = flag.String("output-csv", "", "Write ground truth to CSV file") + provision = flag.Bool("provision", false, "Auto-provision via POST /api/provision") ) // CSIFrame represents a CSI binary frame @@ -155,23 +164,67 @@ func main() { *spaceHeight = height } - if *seed != 0 { - rand.Seed(*seed) + // Parse wall definitions + walls := parseWalls(*wallDefs) + + // Create simulation configuration + config := SimConfig{ + MothershipURL: *mothershipURL, + Token: *token, + Nodes: *nodes, + Walkers: *walkers, + Rate: *rate, + Duration: *duration, + EnableBLE: *enableBLE, + Seed: *seed, + SpaceWidth: *spaceWidth, + SpaceDepth: *spaceDepth, + SpaceHeight: *spaceHeight, + NoiseSigma: *noiseSigma, + VerifyMode: *verifyMode, + OutputCSV: *outputCSV, + Walls: walls, + Verbose: *verbose, + ShowFrameRate: *showFrameRate, + } + + if err := runSimulation(config); err != nil { + log.Fatalf("[ERROR] Simulation failed: %v", err) + } +} + +// runSimulation runs the complete simulation workflow +func runSimulation(config SimConfig) error { + if config.Seed != 0 { + rand.Seed(config.Seed) } log.Printf("[INFO] CSI Simulator starting") - log.Printf("[INFO] Configuration: nodes=%d, walkers=%d, rate=%d Hz, duration=%s", *nodes, *walkers, *rate, *duration) - log.Printf("[INFO] Space: %.1fx%.1fx%.1f m", *spaceWidth, *spaceDepth, *spaceHeight) - if *token != "" { + log.Printf("[INFO] Configuration: nodes=%d, walkers=%d, rate=%d Hz, duration=%s", + config.Nodes, config.Walkers, config.Rate, config.Duration) + log.Printf("[INFO] Space: %.1fx%.1fx%.1f m", config.SpaceWidth, config.SpaceDepth, config.SpaceHeight) + if len(config.Walls) > 0 { + log.Printf("[INFO] Walls: %d defined", len(config.Walls)) + } + if config.Token != "" { log.Printf("[INFO] Using authentication token") } - log.Printf("[INFO] Connecting to: %s", *mothershipURL) + log.Printf("[INFO] Connecting to: %s", config.MothershipURL) + + // Create walker simulator + walkerSim := NewWalkerSimulator(config.Walkers, config.SpaceWidth, config.SpaceDepth, config.SpaceHeight, config.Seed) + + // Open CSV file if specified + if config.OutputCSV != "" { + if err := walkerSim.OpenCSV(config.OutputCSV); err != nil { + return fmt.Errorf("failed to open CSV file: %w", err) + } + defer walkerSim.CloseCSV() + log.Printf("[INFO] Writing ground truth to %s", config.OutputCSV) + } // Create virtual nodes at corners and edges of the room - virtualNodes := createVirtualNodes(*nodes, *spaceWidth, *spaceDepth, *spaceHeight) - - // Create walkers - walkers := createWalkers(*walkers, *spaceWidth, *spaceDepth, *spaceHeight) + virtualNodes := createVirtualNodes(config.Nodes, config.SpaceWidth, config.SpaceDepth, config.SpaceHeight) // Start all nodes var wg sync.WaitGroup @@ -179,7 +232,7 @@ func main() { wg.Add(1) go func(n *VirtualNode) { defer wg.Done() - if err := n.run(walkers, *rate, *duration, *enableBLE, *verbose); err != nil { + if err := n.run(config, walkerSim); err != nil { log.Printf("[ERROR] Node %s failed: %v", n.mac, err) os.Exit(1) } @@ -196,6 +249,54 @@ func main() { log.Printf("[STATS] Node %s: sent %d frames", n.mac, n.frameCount) } } + + // Verification mode: check blob count + if config.VerifyMode { + log.Printf("[INFO] Running verification mode...") + + // Get walker positions + walkerPositions := make([][3]float64, walkerSim.Count()) + for i, w := range walkerSim.GetWalkers() { + walkerPositions[i] = w.Position + } + + // Run verification + verifier := NewVerifier(config.MothershipURL) + result, err := verifier.Verify(walkerSim.Count(), walkerPositions) + if err != nil { + log.Printf("[ERROR] Verification failed: %v", err) + os.Exit(1) + } + + verifier.ExitWithResult(result) + } +} + +// SimConfig holds simulation configuration +type SimConfig struct { + MothershipURL string + Token string + Nodes int + Walkers int + Rate int + Duration time.Duration + EnableBLE bool + Seed int64 + SpaceWidth float64 + SpaceDepth float64 + SpaceHeight float64 + NoiseSigma float64 + VerifyMode bool + OutputCSV string + Walls []WallDef + Verbose bool + ShowFrameRate bool +} + +// WallDef defines a wall segment +type WallDef struct { + X1, Y1, X2, Y2 float64 +} } // createVirtualNodes positions virtual nodes in the space @@ -255,18 +356,55 @@ func createWalkers(count int, width, depth, height float64) []Walker { return walkers } +// parseWalls parses wall definitions from command line flag +func parseWalls(wallDefs string) []WallDef { + if wallDefs == "" { + return nil + } + + var walls []WallDef + parts := strings.Split(wallDefs, ",") + if len(parts) < 4 { + log.Printf("[WARN] Invalid wall definition: %s (expected x1,y1,x2,y2)", wallDefs) + return nil + } + + var x1, y1, x2, y2 float64 + var err error + if x1, err = strconv.ParseFloat(parts[0], 64); err != nil { + log.Printf("[WARN] Invalid wall x1: %v", err) + return nil + } + if y1, err = strconv.ParseFloat(parts[1], 64); err != nil { + log.Printf("[WARN] Invalid wall y1: %v", err) + return nil + } + if x2, err = strconv.ParseFloat(parts[2], 64); err != nil { + log.Printf("[WARN] Invalid wall x2: %v", err) + return nil + } + if y2, err = strconv.ParseFloat(parts[3], 64); err != nil { + log.Printf("[WARN] Invalid wall y2: %v", err) + return nil + } + + walls = append(walls, WallDef{X1: x1, Y1: y1, X2: x2, Y2: y2}) + log.Printf("[INFO] Added wall: (%.1f,%.1f) to (%.1f,%.1f)", x1, y1, x2, y2) + return walls +} + // run starts the virtual node simulation -func (n *VirtualNode) run(walkers []Walker, rateHz int, duration time.Duration, enableBLE, verbose bool) error { +func (n *VirtualNode) run(config SimConfig, walkerSim *WalkerSimulator) error { // Parse mothership URL - u, err := url.Parse(*mothershipURL) + u, err := url.Parse(config.MothershipURL) if err != nil { return fmt.Errorf("invalid mothership URL: %w", err) } // Prepare WebSocket request headers with authentication token headers := make(map[string][]string) - if *token != "" { - headers["X-Spaxel-Token"] = []string{*token} + if config.Token != "" { + headers["X-Spaxel-Token"] = []string{config.Token} } // Connect to mothership @@ -314,7 +452,7 @@ func (n *VirtualNode) run(walkers []Walker, rateHz int, duration time.Duration, return fmt.Errorf("failed to send hello: %w", err) } - if verbose { + if config.Verbose { log.Printf("[DEBUG] Node %s sent hello", n.mac) } @@ -322,7 +460,7 @@ func (n *VirtualNode) run(walkers []Walker, rateHz int, duration time.Duration, time.Sleep(100 * time.Millisecond) // Start ticker for CSI frames - ticker := time.NewTicker(time.Second / time.Duration(rateHz)) + ticker := time.NewTicker(time.Second / time.Duration(config.Rate)) defer ticker.Stop() // Health ticker (every 10 seconds) @@ -331,33 +469,42 @@ func (n *VirtualNode) run(walkers []Walker, rateHz int, duration time.Duration, // BLE ticker (every 5 seconds) var bleTicker *time.Ticker - if enableBLE { + if config.EnableBLE { bleTicker = time.NewTicker(5 * time.Second) defer bleTicker.Stop() } // Frame rate tracking ticker var frameRateTicker *time.Ticker - if *showFrameRate { + if config.ShowFrameRate { frameRateTicker = time.NewTicker(time.Second) defer frameRateTicker.Stop() } startTime := time.Now() frameIndex := uint64(0) + lastCSVWrite := startTime // Main loop (run forever if duration is 0) - for duration == 0 || time.Since(startTime) < duration { + for config.Duration == 0 || time.Since(startTime) < config.Duration { select { case <-ticker.C: // Update walker positions - for i := range walkers { - updateWalkerPosition(&walkers[i], *spaceWidth, *spaceDepth) + walkerSim.Update(0.05) // 50ms step + + // Write CSV row if configured (every 100ms) + if config.OutputCSV != "" && time.Since(lastCSVWrite) > 100*time.Millisecond { + for _, w := range walkerSim.GetWalkers() { + if err := walkerSim.WriteCSVRow(time.Now(), w); err != nil { + log.Printf("[WARN] Failed to write CSV: %v", err) + } + } + lastCSVWrite = time.Now() } // Generate and send CSI frames for each link - for _, walker := range walkers { - frame := n.generateCSIFrame(walker, frameIndex) + for _, walker := range walkerSim.GetWalkers() { + frame := n.generateCSIFrame(walker, frameIndex, config.NoiseSigma) if err := conn.WriteMessage(websocket.BinaryMessage, frame); err != nil { return fmt.Errorf("failed to send CSI frame: %w", err) } @@ -377,7 +524,7 @@ func (n *VirtualNode) run(walkers []Walker, rateHz int, duration time.Duration, WifiRSSIdBm: -50 - rand.Intn(20), // -50 to -70 UptimeMS: uptime, TemperatureC: 40 + rand.Float64()*5, - CSIRateHz: rateHz, + CSIRateHz: config.Rate, WifiChannel: 6, IP: "192.168.1.100", } @@ -392,12 +539,13 @@ func (n *VirtualNode) run(walkers []Walker, rateHz int, duration time.Duration, return fmt.Errorf("failed to send health: %w", err) } - if verbose { + if config.Verbose { log.Printf("[DEBUG] Node %s sent health", n.mac) } case <-bleTicker.C: // Send BLE scan results + walkers := walkerSim.GetWalkers() if len(walkers) > 0 { walker := walkers[0] // Use first walker's BLE ble := BLEMessage{ @@ -406,7 +554,7 @@ func (n *VirtualNode) run(walkers []Walker, rateHz int, duration time.Duration, TimestampMS: time.Now().UnixMilli(), Devices: []BLEDevice{ { - Addr: walker.mac, + Addr: walker.BLEAddress, AddrType: "public", RSSIdBm: -60 - rand.Intn(20), Name: "SimPhone", @@ -425,7 +573,7 @@ func (n *VirtualNode) run(walkers []Walker, rateHz int, duration time.Duration, return fmt.Errorf("failed to send BLE: %w", err) } - if verbose { + if config.Verbose { log.Printf("[DEBUG] Node %s sent BLE scan", n.mac) } } @@ -458,24 +606,32 @@ func (n *VirtualNode) run(walkers []Walker, rateHz int, duration time.Duration, } // generateCSIFrame creates a synthetic CSI frame based on walker position -func (n *VirtualNode) generateCSIFrame(walker Walker, frameIndex uint64) []byte { +func (n *VirtualNode) generateCSIFrame(walker *Walker, frameIndex uint64, noiseSigma float64) []byte { nSub := DefaultSubcarriers // Calculate distance to walker - dx := walker.position[0] - n.position[0] - dy := walker.position[1] - n.position[1] - dz := walker.position[2] - n.position[2] + dx := walker.Position[0] - n.position[0] + dy := walker.Position[1] - n.position[1] + dz := walker.Position[2] - n.position[2] distance := math.Sqrt(dx*dx + dy*dy + dz*dz) // Calculate path loss for RSSI // Free space path loss: PL(d) = PL_0 + 10*n*log10(d/d_0) // PL_0 = 40 dB at d_0 = 1m, n = 2.0 - pathLoss := 40 + 20*math.Log10(distance/1.0) + pathLoss := 40.0 + 20.0*math.Log10(distance/1.0) rssi := int8(-30 - pathLoss) // -30 dBm reference + // Clamp RSSI to valid range + if rssi < -90 { + rssi = -90 + } + if rssi > -30 { + rssi = -30 + } + // Add Fresnel zone modulation // When walker is in a Fresnel zone, amplitude increases - fresnelMod := fresnelModulation(n.position, walker.position) + fresnelMod := fresnelModulation(n.position, walker.Position) // Create frame buf := make([]byte, HeaderSize + nSub*2) @@ -489,7 +645,7 @@ func (n *VirtualNode) generateCSIFrame(walker Walker, frameIndex uint64) []byte copy(buf[6:12], peerMAC[:]) // Timestamp (8 bytes, uint64, little-endian) - timestampUS := uint64(frameIndex * 1_000_000 / uint64(*rate)) + timestampUS := uint64(frameIndex * 1_000_000 / 20) // Assume 20 Hz binary.LittleEndian.PutUint64(buf[12:20], timestampUS) // RSSI (1 byte, int8) @@ -512,16 +668,30 @@ func (n *VirtualNode) generateCSIFrame(walker Walker, frameIndex uint64) []byte // Add subcarrier-dependent phase phase := float64(k) * 0.2 - // Add noise - noise := rand.NormFloat64() * 2.0 + // Add noise with configurable sigma + noise := rand.NormFloat64() * noiseSigma * 100.0 // Scale up for visibility - // Convert to I, Q - iVal := int8(amplitude*math.Cos(phase) + noise) - qVal := int8(amplitude*math.Sin(phase) + noise) + // Convert to I, Q and clamp to int8 range + iVal := amplitude*math.Cos(phase) + noise + qVal := amplitude*math.Sin(phase) + noise + + // Clamp to int8 range + if iVal > 127 { + iVal = 127 + } + if iVal < -127 { + iVal = -127 + } + if qVal > 127 { + qVal = 127 + } + if qVal < -127 { + qVal = -127 + } offset := HeaderSize + k*2 - buf[offset] = byte(iVal) - buf[offset+1] = byte(qVal) + buf[offset] = byte(int8(iVal)) + buf[offset+1] = byte(int8(qVal)) } return buf diff --git a/mothership/cmd/sim/main_test.go b/mothership/cmd/sim/main_test.go index 4289d36..f3ecae7 100644 --- a/mothership/cmd/sim/main_test.go +++ b/mothership/cmd/sim/main_test.go @@ -2,9 +2,14 @@ package main import ( + "bytes" "encoding/binary" "math" + "math/rand" + "os" + "strings" "testing" + "time" ) func TestParseSpaceDims(t *testing.T) { @@ -476,3 +481,470 @@ func bytesEqual(a, b []byte) bool { } return true } + +// TestHelloMessageFormat tests that the hello message matches expected format +func TestHelloMessageFormat(t *testing.T) { + node := &VirtualNode{ + mac: "AA:BB:CC:DD:EE:FF", + position: [3]float64{1.0, 2.0, 2.5}, + } + + // Create a hello message as the node would send it + hello := HelloMessage{ + Type: "hello", + MAC: node.mac, + NodeID: fmt.Sprintf("sim-node-%s", node.mac), + FirmwareVersion: "0.1.0-sim", + Capabilities: []string{"csi", "tx", "rx"}, + Chip: "ESP32-S3", + FlashMB: 16, + UptimeMS: 1000, + } + + // Marshal to JSON + data, err := json.Marshal(hello) + if err != nil { + t.Fatalf("Failed to marshal hello: %v", err) + } + + // Unmarshal and verify + var decoded HelloMessage + if err := json.Unmarshal(data, &decoded); err != nil { + t.Fatalf("Failed to unmarshal hello: %v", err) + } + + if decoded.Type != "hello" { + t.Errorf("Type = %s, want 'hello'", decoded.Type) + } + if decoded.MAC != node.mac { + t.Errorf("MAC = %s, want %s", decoded.MAC, node.mac) + } + if decoded.FirmwareVersion != "0.1.0-sim" { + t.Errorf("FirmwareVersion = %s, want '0.1.0-sim'", decoded.FirmwareVersion) + } + if len(decoded.Capabilities) != 3 { + t.Errorf("Capabilities length = %d, want 3", len(decoded.Capabilities)) + } +} + +// TestIQClamping tests that I/Q values are clamped to int8 range +func TestIQClamping(t *testing.T) { + tests := []struct { + name string + amplitude float64 + phase float64 + noiseSigma float64 + wantInRange bool + }{ + { + name: "normal values", + amplitude: 30.0, + phase: 0.5, + noiseSigma: 0.005, + wantInRange: true, + }, + { + name: "high amplitude", + amplitude: 500.0, + phase: 0.0, + noiseSigma: 0.005, + wantInRange: true, // Should be clamped + }, + { + name: "negative amplitude", + amplitude: -100.0, + phase: 0.0, + noiseSigma: 0.005, + wantInRange: true, // Should be clamped + }, + { + name: "large noise", + amplitude: 30.0, + phase: 0.5, + noiseSigma: 1.0, + wantInRange: true, // Should be clamped + }, + } + + for _, tt := range tests { + t.Run(tt.name, func(t *testing.T) { + // Generate I/Q pair + noise := rand.NormFloat64() * tt.noiseSigma * 100.0 + iVal := tt.amplitude*math.Cos(tt.phase) + noise + qVal := tt.amplitude*math.Sin(tt.phase) + noise + + // Clamp to int8 range + if iVal > 127 { + iVal = 127 + } + if iVal < -127 { + iVal = -127 + } + if qVal > 127 { + qVal = 127 + } + if qVal < -127 { + qVal = -127 + } + + // Check range + inRange := int8(iVal) >= -127 && int8(iVal) <= 127 && + int8(qVal) >= -127 && int8(qVal) <= 127 + + if inRange != tt.wantInRange { + t.Errorf("I/Q in range = %v, want %v (I=%d, Q=%d)", + inRange, tt.wantInRange, int8(iVal), int8(qVal)) + } + }) + } +} + +// TestSeedReproducibility tests that --seed produces identical walker paths +func TestSeedReproducibility(t *testing.T) { + seed := int64(42) + width := 6.0 + depth := 5.0 + height := 2.5 + + // First run with seed + rand.Seed(seed) + walkerSim1 := NewWalkerSimulator(1, width, depth, height, seed) + + // Update positions + walkerSim1.Update(1.0) // 1 second + pos1 := walkerSim1.GetWalkers()[0].Position + + // Reset and run again with same seed + rand.Seed(seed) + walkerSim2 := NewWalkerSimulator(1, width, depth, height, seed) + + walkerSim2.Update(1.0) + pos2 := walkerSim2.GetWalkers()[0].Position + + // Positions should be identical + if pos1[0] != pos2[0] || pos1[1] != pos2[1] || pos1[2] != pos2[2] { + t.Errorf("Positions differ with same seed: run1=%v, run2=%v", pos1, pos2) + } +} + +// TestOutputCSV tests that --output-csv generates a CSV with correct headers +func TestOutputCSV(t *testing.T) { + width := 6.0 + depth := 5.0 + height := 2.5 + seed := int64(42) + + // Create a temporary CSV file + tmpFile, err := os.CreateTemp("", "sim-test-*.csv") + if err != nil { + t.Fatalf("Failed to create temp file: %v", err) + } + defer os.Remove(tmpFile.Name()) + tmpFile.Close() + + // Create walker simulator + walkerSim := NewWalkerSimulator(1, width, depth, height, seed) + + // Open CSV + if err := walkerSim.OpenCSV(tmpFile.Name()); err != nil { + t.Fatalf("Failed to open CSV: %v", err) + } + defer walkerSim.CloseCSV() + + // Write some data rows + timestamp := time.Date(2026, 4, 9, 12, 0, 0, 0, time.UTC) + walker := walkerSim.GetWalkers()[0] + + for i := 0; i < 3; i++ { + timestamp = timestamp.Add(time.Second) + if err := walkerSim.WriteCSVRow(timestamp, walker); err != nil { + t.Fatalf("Failed to write CSV row: %v", err) + } + } + + // Flush and read back + if err := walkerSim.CloseCSV(); err != nil { + t.Fatalf("Failed to close CSV: %v", err) + } + + // Read and verify CSV content + data, err := os.ReadFile(tmpFile.Name()) + if err != nil { + t.Fatalf("Failed to read CSV file: %v", err) + } + + lines := strings.Split(string(data), "\n") + + // Check header + expectedHeader := "timestamp_ms,walker_id,x,y,z,vx,vy,vz" + if lines[0] != expectedHeader { + t.Errorf("CSV header = %s, want %s", lines[0], expectedHeader) + } + + // Check we have 4 lines (header + 3 data rows) + if len(lines) < 4 { + t.Errorf("CSV has %d lines, want at least 4", len(lines)) + } + + // Verify data row format + dataLine := strings.Split(lines[1], ",") + if len(dataLine) != 8 { + t.Errorf("Data row has %d columns, want 8", len(dataLine)) + } +} + +// TestVerificationModeBlobCount tests that --verify correctly detects missing blobs +func TestVerificationModeBlobCount(t *testing.T) { + // This test would require a running mothership server + // For now, we test the verification logic in isolation + + verifier := NewVerifier("http://localhost:8080") + + // Test allWalkersInBounds with default bounds + tests := []struct { + name string + positions [][3]float64 + want bool + }{ + { + name: "all in bounds", + positions: [][3]float64{{3, 2.5, 1.7}, {1, 1, 1}}, + want: true, + }, + { + name: "one out of bounds (X)", + positions: [][3]float64{{7, 2.5, 1.7}, {3, 2.5, 1.7}}, + want: false, + }, + { + name: "one out of bounds (Y)", + positions: [][3]float64{{3, 6, 1.7}, {3, 2.5, 1.7}}, + want: false, + }, + { + name: "one out of bounds (Z)", + positions: [][3]float64{{3, 2.5, 3}, {3, 2.5, 1.7}}, + want: false, + }, + } + + for _, tt := range tests { + t.Run(tt.name, func(t *testing.T) { + result := verifier.allWalkersInBounds(tt.positions) + if result != tt.want { + t.Errorf("allWalkersInBounds() = %v, want %v", result, tt.want) + } + }) + } +} + +// TestVerifyBlobDetection tests that verify correctly detects blob count mismatches +func TestVerifyBlobDetection(t *testing.T) { + verifier := NewVerifier("http://localhost:8080") + + // Test walkersHaveNearbyBlobs + walkerPositions := [][3]float64{{3, 2.5, 1.7}, {1, 1, 1}} + + tests := []struct { + name string + blobs []Blob + want bool + }{ + { + name: "blob near each walker", + blobs: []Blob{ + {X: 3.0, Y: 2.5, Z: 1.7}, + {X: 1.0, Y: 1.0, Z: 1.0}, + }, + want: true, + }, + { + name: "blob too far from first walker", + blobs: []Blob{ + {X: 5.5, Y: 2.5, Z: 1.7}, + }, + want: false, + }, + { + name: "no blobs", + blobs: []Blob{}, + want: false, + }, + } + + for _, tt := range tests { + t.Run(tt.name, func(t *testing.T) { + result := verifier.walkersHaveNearbyBlobs(walkerPositions, tt.blobs) + if result != tt.want { + t.Errorf("walkersHaveNearbyBlobs() = %v, want %v", result, tt.want) + } + }) + } +} + +// TestDistance3D tests the 3D distance calculation +func TestDistance3D(t *testing.T) { + tests := []struct { + name string + a [3]float64 + b [3]float64 + wantDist float64 + }{ + { + name: "same point", + a: [3]float64{0, 0, 0}, + b: [3]float64{0, 0, 0}, + wantDist: 0, + }, + { + name: "unit distance on X", + a: [3]float64{0, 0, 0}, + b: [3]float64{1, 0, 0}, + wantDist: 1.0, + }, + { + name: "3D distance", + a: [3]float64{0, 0, 0}, + b: [3]float64{3, 4, 0}, + wantDist: 5.0, + }, + { + name: "with Z component", + a: [3]float64{0, 0, 0}, + b: [3]float64{0, 0, 2}, + wantDist: 2.0, + }, + } + + for _, tt := range tests { + t.Run(tt.name, func(t *testing.T) { + got := distance3D(tt.a, tt.b) + if math.Abs(got-tt.wantDist) > 1e-6 { + t.Errorf("distance3D() = %f, want %f", got, tt.wantDist) + } + }) + } +} + +// TestWallParsing tests wall definition parsing +func TestWallParsing(t *testing.T) { + tests := []struct { + name string + input string + wantCount int + wantValid bool + }{ + { + name: "valid wall", + input: "1,2,3,4", + wantCount: 1, + wantValid: true, + }, + { + name: "valid wall with decimals", + input: "1.5,2.5,3.5,4.5", + wantCount: 1, + wantValid: true, + }, + { + name: "invalid - missing coordinate", + input: "1,2,3", + wantCount: 0, + wantValid: false, + }, + { + name: "invalid - non-numeric", + input: "a,b,c,d", + wantCount: 0, + wantValid: false, + }, + } + + for _, tt := range tests { + t.Run(tt.name, func(t *testing.T) { + walls := parseWalls(tt.input) + if tt.wantValid { + if len(walls) != tt.wantCount { + t.Errorf("parseWalls() returned %d walls, want %d", len(walls), tt.wantCount) + } + } else { + if len(walls) != 0 { + t.Errorf("parseWalls() should have returned empty for invalid input, got %d walls", len(walls)) + } + } + }) + } +} + +// TestBinaryHeaderFormat tests that CSI frames have correct binary header format +func TestBinaryHeaderFormat(t *testing.T) { + node := &VirtualNode{ + mac: "AA:BB:CC:DD:EE:FF", + position: [3]float64{0, 0, 2.5}, + } + + walker := &Walker{ + Position: [3]float64{3, 2.5, 1.7}, + Velocity: [3]float64{0.1, 0, 0}, + mac: "11:22:33:44:55:00", + } + + frame := node.generateCSIFrame(walker, 0, 0.005) + + // Check minimum frame length + if len(frame) < HeaderSize { + t.Fatalf("Frame length %d is less than header size %d", len(frame), HeaderSize) + } + + // Check magic number (if we had one at the start) + // For now, check that node MAC is in correct position + nodeMAC := frame[0:6] + expectedMAC := macToBytes(node.mac) + if !bytesEqual(nodeMAC, expectedMAC[:]) { + t.Errorf("Node MAC = %v, want %v", nodeMAC, expectedMAC) + } + + // Check peer MAC is in correct position + peerMAC := frame[6:12] + if len(peerMAC) != 6 { + t.Errorf("Peer MAC length = %d, want 6", len(peerMAC)) + } + + // Check timestamp is at correct position and is reasonable + timestampUS := binary.LittleEndian.Uint64(frame[12:20]) + if timestampUS > 100000 { // Should be less than 100ms for first frame + t.Errorf("Timestamp = %d us, want < 100000 us for first frame", timestampUS) + } + + // Check RSSI is in valid range + rssi := int8(frame[20]) + if rssi < -90 || rssi > -30 { + t.Errorf("RSSI = %d dBm, want between -90 and -30", rssi) + } + + // Check noise floor + noiseFloor := int8(frame[21]) + if noiseFloor < -100 || noiseFloor > -50 { + t.Errorf("Noise floor = %d dBm, want between -100 and -50", noiseFloor) + } + + // Check channel is valid WiFi channel + channel := frame[22] + if channel < 1 || channel > 14 { + t.Errorf("Channel = %d, want valid channel 1-14", channel) + } + + // Check number of subcarriers + nSub := frame[23] + if nSub < 1 || nSub > MaxSubcarriers { + t.Errorf("Subcarriers = %d, want between 1 and %d", nSub, MaxSubcarriers) + } + + // Verify payload length matches nSub + expectedPayloadSize := int(nSub) * 2 + expectedFrameSize := HeaderSize + expectedPayloadSize + if len(frame) != expectedFrameSize { + t.Errorf("Frame length = %d, want %d (header %d + payload %d)", + len(frame), expectedFrameSize, HeaderSize, expectedPayloadSize) + } +} diff --git a/mothership/cmd/sim/verify.go b/mothership/cmd/sim/verify.go new file mode 100644 index 0000000..1f70cd5 --- /dev/null +++ b/mothership/cmd/sim/verify.go @@ -0,0 +1,300 @@ +// Package main provides verification logic for the CSI simulator. +package main + +import ( + "encoding/json" + "fmt" + "io" + "math" + "net/http" + "os" + "time" +) + +// VerificationResult represents the result of a verification check +type VerificationResult struct { + Passed bool `json:"passed"` + Message string `json:"message"` + BlobCount int `json:"blob_count"` + WalkerCount int `json:"walker_count"` + ExpectedBlobs int `json:"expected_blobs"` + BlobDetails []Blob `json:"blob_details"` + Timestamp time.Time `json:"timestamp"` +} + +// Blob represents a detected blob from the mothership API +type Blob struct { + ID int `json:"id"` + X float64 `json:"x"` + Y float64 `json:"y"` + Z float64 `json:"z"` + Confidence float64 `json:"confidence"` + Person string `json:"person,omitempty"` +} + +// Verifier handles blob count verification +type Verifier struct { + mothershipURL string + httpClient *http.Client + tolerance int // ±1 tolerance for blob count + distance float64 // Max distance for walker-blob matching (meters) +} + +// NewVerifier creates a new verifier +func NewVerifier(mothershipURL string) *Verifier { + return &Verifier{ + mothershipURL: mothershipURL, + httpClient: &http.Client{ + Timeout: 10 * time.Second, + }, + tolerance: 1, + distance: 2.0, + } +} + +// SetTolerance sets the blob count tolerance +func (v *Verifier) SetTolerance(tolerance int) { + v.tolerance = tolerance +} + +// SetDistance sets the max distance for walker-blob matching +func (v *Verifier) SetDistance(distance float64) { + v.distance = distance +} + +// Verify performs the verification check +func (v *Verifier) Verify(walkerCount int, walkerPositions [][3]float64) (*VerificationResult, error) { + // Wait 2 seconds for pipeline to settle + time.Sleep(2 * time.Second) + + // Fetch current blobs from mothership + blobs, err := v.fetchBlobs() + if err != nil { + return nil, fmt.Errorf("failed to fetch blobs: %w", err) + } + + result := &VerificationResult{ + BlobCount: len(blobs), + WalkerCount: walkerCount, + ExpectedBlobs: walkerCount, + BlobDetails: blobs, + Timestamp: time.Now(), + } + + // Check blob count against walker count (with tolerance) + minExpected := walkerCount - v.tolerance + maxExpected := walkerCount + v.tolerance + + if len(blobs) < minExpected { + result.Passed = false + result.Message = fmt.Sprintf("FAIL: expected %d±%d blobs, got %d", + walkerCount, v.tolerance, len(blobs)) + return result, nil + } + + if len(blobs) > maxExpected { + result.Passed = false + result.Message = fmt.Sprintf("FAIL: expected %d±%d blobs, got %d", + walkerCount, v.tolerance, len(blobs)) + return result, nil + } + + // If walkers are within room bounds, check all walkers have a blob within distance + if v.allWalkersInBounds(walkerPositions) { + if !v.walkersHaveNearbyBlobs(walkerPositions, blobs) { + result.Passed = false + result.Message = fmt.Sprintf("FAIL: %d blobs detected but not all walkers within %.1fm", + len(blobs), v.distance) + return result, nil + } + } + + result.Passed = true + result.Message = fmt.Sprintf("PASS: %d blobs detected for %d walkers", + len(blobs), walkerCount) + + return result, nil +} + +// fetchBlobs fetches the current list of blobs from the mothership +func (v *Verifier) fetchBlobs() ([]Blob, error) { + // Convert ws:// URL to http:// for REST API + apiURL := wsToHTTP(v.mothershipURL) + "/api/blobs" + + resp, err := v.httpClient.Get(apiURL) + if err != nil { + return nil, fmt.Errorf("HTTP request failed: %w", err) + } + defer resp.Body.Close() + + if resp.StatusCode != http.StatusOK { + body, _ := io.ReadAll(resp.Body) + return nil, fmt.Errorf("HTTP %d: %s", resp.StatusCode, string(body)) + } + + body, err := io.ReadAll(resp.Body) + if err != nil { + return nil, fmt.Errorf("failed to read response: %w", err) + } + + var blobs []Blob + if err := json.Unmarshal(body, &blobs); err != nil { + return nil, fmt.Errorf("failed to parse JSON: %w", err) + } + + return blobs, nil +} + +// allWalkersInBounds checks if all walkers are within room bounds +func (v *Verifier) allWalkersInBounds(positions [][3]float64) bool { + // Assume room bounds from space dimensions + // Default bounds: 0-6m X, 0-5m Y, 0-2.5m Z + for _, pos := range positions { + if pos[0] < 0 || pos[0] > 6 || pos[1] < 0 || pos[1] > 5 || pos[2] < 0 || pos[2] > 2.5 { + return false + } + } + return true +} + +// walkersHaveNearbyBlobs checks if each walker has a blob within distance +func (v *Verifier) walkersHaveNearbyBlobs(walkerPositions [][3]float64, blobs []Blob) bool { + for _, walkerPos := range walkerPositions { + hasNearbyBlob := false + for _, blob := range blobs { + dist := distance3D(walkerPos, [3]float64{blob.X, blob.Y, blob.Z}) + if dist <= v.distance { + hasNearbyBlob = true + break + } + } + if !hasNearbyBlob { + return false + } + } + return true +} + +// distance3D computes 3D Euclidean distance +func distance3D(a, b [3]float64) float64 { + dx := a[0] - b[0] + dy := a[1] - b[1] + dz := a[2] - b[2] + return math.Sqrt(dx*dx + dy*dy + dz*dz) +} + +// wsToHTTP converts a WebSocket URL to HTTP URL +func wsToHTTP(wsURL string) string { + // Simple string replacement + // ws://localhost:8080/ws/node -> http://localhost:8080 + if len(wsURL) >= 5 && wsURL[:5] == "ws://" { + return "http://" + wsURL[5:] + } + if len(wsURL) >= 6 && wsURL[:6] == "wss://" { + return "https://" + wsURL[6:] + } + + // If URL doesn't start with ws:// or wss://, assume it's already http/https + return wsURL +} + +// PrintResult prints the verification result to stdout +func (v *Verifier) PrintResult(result *VerificationResult) { + fmt.Printf("\n=== Verification Result ===\n") + fmt.Printf("Status: %s\n", result.Message) + fmt.Printf("Blob count: %d\n", result.BlobCount) + fmt.Printf("Walker count: %d\n", result.WalkerCount) + fmt.Printf("Expected blobs: %d±%d\n", result.WalkerCount, v.tolerance) + fmt.Printf("Timestamp: %s\n", result.Timestamp.Format(time.RFC3339)) + + if len(result.BlobDetails) > 0 { + fmt.Printf("\nBlob details:\n") + for _, blob := range result.BlobDetails { + person := blob.Person + if person == "" { + person = "Unknown" + } + fmt.Printf(" ID %d: (%.2f, %.2f, %.2f) confidence=%.2f person=%s\n", + blob.ID, blob.X, blob.Y, blob.Z, blob.Confidence, person) + } + } + fmt.Printf("============================\n\n") +} + +// ExitWithResult exits with appropriate exit code based on verification result +func (v *Verifier) ExitWithResult(result *VerificationResult) { + v.PrintResult(result) + + if result.Passed { + os.Exit(0) + } + os.Exit(1) +} + +// RunVerification runs the full verification workflow +func RunVerification(mothershipURL string, walkerCount int, walkerPositions [][3]float64, tolerance int) error { + verifier := NewVerifier(mothershipURL) + verifier.SetTolerance(tolerance) + + result, err := verifier.Verify(walkerCount, walkerPositions) + if err != nil { + return fmt.Errorf("verification failed: %w", err) + } + + verifier.ExitWithResult(result) + return nil +} + +// ProvisionToken attempts to provision a new token from the mothership +func ProvisionToken(mothershipURL string) (string, error) { + apiURL := wsToHTTP(mothershipURL) + "/api/provision" + + // Create request body with synthetic credentials + reqBody := []byte(`{"mac":"AA:BB:CC:DD:EE:FF"}`) + + resp, err := http.Post(apiURL, "application/json", nil) + if err != nil { + return "", fmt.Errorf("provision request failed: %w", err) + } + defer resp.Body.Close() + + if resp.StatusCode != http.StatusOK { + body, _ := io.ReadAll(resp.Body) + return "", fmt.Errorf("provision returned HTTP %d: %s", resp.StatusCode, string(body)) + } + + // Read response - this should return the token + token, err := io.ReadAll(resp.Body) + if err != nil { + return "", fmt.Errorf("failed to read token: %w", err) + } + + return string(token), nil +} + +// GetMothershipStatus fetches the mothership status +func GetMothershipStatus(mothershipURL string) (map[string]interface{}, error) { + apiURL := wsToHTTP(mothershipURL) + "/api/status" + + resp, err := http.Get(apiURL) + if err != nil { + return nil, fmt.Errorf("status request failed: %w", err) + } + defer resp.Body.Close() + + if resp.StatusCode != http.StatusOK { + return nil, fmt.Errorf("status returned HTTP %d", resp.StatusCode) + } + + body, err := io.ReadAll(resp.Body) + if err != nil { + return nil, fmt.Errorf("failed to read status: %w", err) + } + + var status map[string]interface{} + if err := json.Unmarshal(body, &status); err != nil { + return nil, fmt.Errorf("failed to parse status: %w", err) + } + + return status, nil +} diff --git a/mothership/cmd/sim/walker.go b/mothership/cmd/sim/walker.go new file mode 100644 index 0000000..f120301 --- /dev/null +++ b/mothership/cmd/sim/walker.go @@ -0,0 +1,386 @@ +// Package main provides walker simulation for the CSI simulator. +package main + +import ( + "encoding/csv" + "fmt" + "math" + "math/rand" + "os" + "time" + + "github.com/spaxel/mothership/internal/simulator" +) + +// WalkerSimulator manages simulated walkers +type WalkerSimulator struct { + walkers []*Walker + spaceWidth float64 + spaceDepth float64 + spaceHeight float64 + rng *rand.Rand + csvFile *os.File + csvWriter *csv.Writer +} + +// Walker represents a simulated person moving through space +type Walker struct { + ID string + Position [3]float64 // x, y, z in meters + Velocity [3]float64 // vx, vy, vz in m/s + BLEAddress string // Simulated BLE device + lastUpdate time.Time + path []*WalkerPathPoint + pathIndex int + seed int64 +} + +// WalkerPathPoint represents a point in a predefined path +type WalkerPathPoint struct { + Position [3]float64 + WaitTime time.Duration +} + +// NewWalkerSimulator creates a new walker simulator +func NewWalkerSimulator(count int, width, depth, height float64, seed int64) *WalkerSimulator { + rng := rand.New(rand.NewSource(seed)) + + walkers := make([]*Walker, count) + for i := 0; i < count; i++ { + walkers[i] = &Walker{ + ID: fmt.Sprintf("walker-%d", i), + Position: [3]float64{ + width/2 + (rng.Float64()-0.5)*width*0.5, + depth/2 + (rng.Float64()-0.5)*depth*0.5, + 1.7, // Average person height + }, + Velocity: [3]float64{ + (rng.Float64() - 0.5) * 0.5, + (rng.Float64() - 0.5) * 0.5, + 0, + }, + BLEAddress: fmt.Sprintf("11:22:33:44:55:%02X", i), + lastUpdate: time.Now(), + seed: seed + int64(i), + } + } + + return &WalkerSimulator{ + walkers: walkers, + spaceWidth: width, + spaceDepth: depth, + spaceHeight: height, + rng: rng, + } +} + +// SetPath sets a predefined path for a walker +func (ws *WalkerSimulator) SetPath(walkerIndex int, path [][3]float64) { + if walkerIndex >= 0 && walkerIndex < len(ws.walkers) { + ws.walkers[walkerIndex].path = make([]*WalkerPathPoint, len(path)) + for i, p := range path { + ws.walkers[walkerIndex].path[i] = &WalkerPathPoint{ + Position: p, + WaitTime: 0, + } + } + ws.walkers[walkerIndex].pathIndex = 0 + } +} + +// OpenCSV opens a CSV file for writing ground truth data +func (ws *WalkerSimulator) OpenCSV(filename string) error { + file, err := os.Create(filename) + if err != nil { + return fmt.Errorf("failed to create CSV file: %w", err) + } + + ws.csvFile = file + ws.csvWriter = csv.NewWriter(file) + + // Write header + header := []string{"timestamp_ms", "walker_id", "x", "y", "z", "vx", "vy", "vz"} + if err := ws.csvWriter.Write(header); err != nil { + return fmt.Errorf("failed to write CSV header: %w", err) + } + + return nil +} + +// CloseCSV closes the CSV file +func (ws *WalkerSimulator) CloseCSV() error { + if ws.csvWriter != nil { + ws.csvWriter.Flush() + } + if ws.csvFile != nil { + return ws.csvFile.Close() + } + return nil +} + +// WriteCSVRow writes a row to the CSV file +func (ws *WalkerSimulator) WriteCSVRow(timestamp time.Time, walker *Walker) error { + if ws.csvWriter == nil { + return nil + } + + row := []string{ + fmt.Sprintf("%d", timestamp.UnixMilli()), + walker.ID, + fmt.Sprintf("%.3f", walker.Position[0]), + fmt.Sprintf("%.3f", walker.Position[1]), + fmt.Sprintf("%.3f", walker.Position[2]), + fmt.Sprintf("%.3f", walker.Velocity[0]), + fmt.Sprintf("%.3f", walker.Velocity[1]), + fmt.Sprintf("%.3f", walker.Velocity[2]), + } + + return ws.csvWriter.Write(row) +} + +// Update updates all walker positions +// dt is the time step in seconds +func (ws *WalkerSimulator) Update(dt float64) { + for _, w := range ws.walkers { + ws.updateWalker(w, dt) + } +} + +// updateWalker updates a single walker's position +func (ws *WalkerSimulator) updateWalker(w *Walker, dt float64) { + // If path is defined, follow path + if len(w.path) > 0 { + ws.followPath(w, dt) + return + } + + // Random walk motion + const dtStep = 0.05 // 50ms step + + // Update position + w.Position[0] += w.Velocity[0] * dtStep + w.Position[1] += w.Velocity[1] * dtStep + + // Bounce off walls + margin := 0.2 // 20cm margin + if w.Position[0] < margin { + w.Position[0] = margin + w.Velocity[0] *= -1 + } + if w.Position[0] > ws.spaceWidth-margin { + w.Position[0] = ws.spaceWidth - margin + w.Velocity[0] *= -1 + } + if w.Position[1] < margin { + w.Position[1] = margin + w.Velocity[1] *= -1 + } + if w.Position[1] > ws.spaceDepth-margin { + w.Position[1] = ws.spaceDepth - margin + w.Velocity[1] *= -1 + } + + // Random velocity perturbation + w.Velocity[0] += (ws.rng.Float64() - 0.5) * 0.1 + w.Velocity[1] += (ws.rng.Float64() - 0.5) * 0.1 + + // Clamp velocity + maxSpeed := 0.5 + speed := math.Sqrt(w.Velocity[0]*w.Velocity[0] + w.Velocity[1]*w.Velocity[1]) + if speed > maxSpeed { + scale := maxSpeed / speed + w.Velocity[0] *= scale + w.Velocity[1] *= scale + } + + w.lastUpdate = time.Now() +} + +// followPath makes a walker follow a predefined path +func (ws *WalkerSimulator) followPath(w *Walker, dt float64) { + if w.pathIndex >= len(w.path) { + w.pathIndex = 0 // Loop back to start + } + + target := w.path[w.pathIndex].Position + + // Vector to target + dx := target[0] - w.Position[0] + dy := target[1] - w.Position[1] + dz := target[2] - w.Position[2] + dist := math.Sqrt(dx*dx + dy*dy + dz*dz) + + // If very close to target, move to next point + if dist < 0.1 { + w.pathIndex++ + return + } + + // Move towards target at constant speed + moveDist := 0.5 * dt // 0.5 m/s + if moveDist > dist { + moveDist = dist + } + + t := moveDist / dist + w.Position[0] += dx * t + w.Position[1] += dy * t + w.Position[2] += dz * t + + // Update velocity vector for consistency + if dist > 0 { + w.Velocity[0] = (dx / dist) * 0.5 + w.Velocity[1] = (dy / dist) * 0.5 + w.Velocity[2] = (dz / dist) * 0.5 + } + + w.lastUpdate = time.Now() +} + +// GetWalkers returns all walkers +func (ws *WalkerSimulator) GetWalkers() []*Walker { + return ws.walkers +} + +// GetWalkerPositions returns walker positions as simulator.Point slice +func (ws *WalkerSimulator) GetWalkerPositions() []simulator.Point { + positions := make([]simulator.Point, len(ws.walkers)) + for i, w := range ws.walkers { + positions[i] = simulator.Point{ + X: w.Position[0], + Y: w.Position[1], + Z: w.Position[2], + } + } + return positions +} + +// GetWalkerByID returns a walker by ID +func (ws *WalkerSimulator) GetWalkerByID(id string) *Walker { + for _, w := range ws.walkers { + if w.ID == id { + return w + } + } + return nil +} + +// Count returns the number of walkers +func (ws *WalkerSimulator) Count() int { + return len(ws.walkers) +} + +// ValidateWalkerPosition checks if a walker position is within bounds +func (ws *WalkerSimulator) ValidateWalkerPosition(pos [3]float64) bool { + return pos[0] >= 0 && pos[0] <= ws.spaceWidth && + pos[1] >= 0 && pos[1] <= ws.spaceDepth && + pos[2] >= 0 && pos[2] <= ws.spaceHeight +} + +// GenerateNodeToNodePath generates a path through node positions +func (ws *WalkerSimulator) GenerateNodeToNodePath(nodePositions [][3]float64) [][3]float64 { + if len(nodePositions) < 2 { + return nodePositions + } + + // Create path that visits each node in order + path := make([][3]float64, len(nodePositions)) + copy(path, nodePositions) + + return path +} + +// GenerateRandomPath generates a random rectangular path around the space +func (ws *WalkerSimulator) GenerateRandomPath(numPoints int) [][3]float64 { + path := make([][3]float64, numPoints) + margin := 0.5 // 50cm margin from walls + + for i := 0; i < numPoints; i++ { + // Generate random positions within bounds + path[i] = [3]float64{ + margin + ws.rng.Float64()*(ws.spaceWidth-2*margin), + margin + ws.rng.Float64()*(ws.spaceDepth-2*margin), + 1.7, // Average person height + } + } + + return path +} + +// GeneratePerimeterPath generates a rectangular path around the space perimeter +func (ws *WalkerSimulator) GeneratePerimeterPath() [][3]float64 { + margin := 0.5 // 50cm margin from walls + + return [][3]float64{ + {margin, margin, 1.7}, + {ws.spaceWidth - margin, margin, 1.7}, + {ws.spaceWidth - margin, ws.spaceDepth - margin, 1.7}, + {margin, ws.spaceDepth - margin, 1.7}, + } +} + +// GetWalkerSpeed returns the current speed of a walker +func (w *Walker) GetSpeed() float64 { + return math.Sqrt(w.Velocity[0]*w.Velocity[0] + w.Velocity[1]*w.Velocity[1] + w.Velocity[2]*w.Velocity[2]) +} + +// IsMoving returns true if the walker is moving (speed > threshold) +func (w *Walker) IsMoving() bool { + return w.GetSpeed() > 0.01 +} + +// GetPositionAsPoint returns walker position as simulator.Point +func (w *Walker) GetPositionAsPoint() simulator.Point { + return simulator.Point{ + X: w.Position[0], + Y: w.Position[1], + Z: w.Position[2], + } +} + +// SetPosition sets the walker's position +func (w *Walker) SetPosition(x, y, z float64) { + w.Position[0] = x + w.Position[1] = y + w.Position[2] = z +} + +// SetVelocity sets the walker's velocity +func (w *Walker) SetVelocity(vx, vy, vz float64) { + w.Velocity[0] = vx + w.Velocity[1] = vy + w.Velocity[2] = vz +} + +// GetDistanceToNode returns distance from walker to a node position +func (w *Walker) GetDistanceToNode(nodePos [3]float64) float64 { + dx := nodePos[0] - w.Position[0] + dy := nodePos[1] - w.Position[1] + dz := nodePos[2] - w.Position[2] + return math.Sqrt(dx*dx + dy*dy + dz*dz) +} + +// Clone creates a deep copy of the walker +func (w *Walker) Clone() *Walker { + clone := &Walker{ + ID: w.ID, + Position: w.Position, + Velocity: w.Velocity, + BLEAddress: w.BLEAddress, + lastUpdate: w.lastUpdate, + pathIndex: w.pathIndex, + seed: w.seed, + } + + if len(w.path) > 0 { + clone.path = make([]*WalkerPathPoint, len(w.path)) + for i, p := range w.path { + clone.path[i] = &WalkerPathPoint{ + Position: p.Position, + WaitTime: p.WaitTime, + } + } + } + + return clone +} diff --git a/mothership/internal/simulator/engine.go b/mothership/internal/simulator/engine.go index 109c12a..a0f3389 100644 --- a/mothership/internal/simulator/engine.go +++ b/mothership/internal/simulator/engine.go @@ -23,9 +23,9 @@ type Engine struct { mu sync.RWMutex space *SpaceDefinition virtualNodes []*VirtualNode - walkers []*Walker + walkers []*EngineWalker grid *Grid - links []*Link + links []*EngineLink publishedResults *SimulationResult subscribers []chan *SimulationResult } @@ -48,7 +48,7 @@ type VirtualNode struct { } // Walker represents a simulated person moving through the space. -type Walker struct { +type EngineWalker struct { ID string `json:"id"` Position [3]float64 `json:"position"` // x, y, z in meters Velocity [3]float64 `json:"velocity"` // vx, vy, vz in m/s @@ -69,7 +69,7 @@ type Grid struct { } // Link represents a virtual WiFi link between two nodes. -type Link struct { +type EngineLink struct { ID string `json:"id"` TXNodeID string `json:"tx_node_id"` RXNodeID string `json:"rx_node_id"` @@ -110,7 +110,7 @@ func NewEngine(space *SpaceDefinition) *Engine { return &Engine{ space: space, virtualNodes: make([]*VirtualNode, 0), - walkers: make([]*Walker, 0), + walkers: make([]*EngineWalker, 0), subscribers: make([]chan *SimulationResult, 0), } } @@ -166,7 +166,7 @@ func (e *Engine) RemoveVirtualNode(id string) { } // AddWalker adds a simulated walker. -func (e *Engine) AddWalker(walker *Walker) { +func (e *Engine) AddWalker(walker *EngineWalker) { e.mu.Lock() defer e.mu.Unlock() @@ -203,12 +203,12 @@ func (e *Engine) GetVirtualNodes() []*VirtualNode { } // GetWalkers returns all walkers. -func (e *Engine) GetWalkers() []*Walker { +func (e *Engine) GetWalkers() []*EngineWalker { e.mu.RLock() defer e.mu.RUnlock() // Return copies - walkers := make([]*Walker, len(e.walkers)) + walkers := make([]*EngineWalker, len(e.walkers)) for i, w := range e.walkers { walkerCopy := *w walkers[i] = &walkerCopy @@ -316,7 +316,7 @@ func (e *Engine) initializeGrid() { // generateLinks creates virtual links between all node pairs. func (e *Engine) generateLinks() { - e.links = make([]*Link, 0) + e.links = make([]*EngineLink, 0) // Create links between all pairs of nodes for i, tx := range e.virtualNodes { @@ -350,7 +350,7 @@ func (e *Engine) generateLinks() { } // computeZoneCache precomputes Fresnel zone numbers for all grid cells. -func (e *Engine) computeZoneCache(link *Link) []*ZoneInfo { +func (e *Engine) computeZoneCache(link *EngineLink) []*ZoneInfo { const lambda = 0.123 // WiFi wavelength halfLambda := lambda / 2 @@ -438,8 +438,8 @@ func (e *Engine) updateWalkers() { } // Random velocity perturbation - walker.Velocity[0] += (randFloat64() - 0.5) * 0.1 - walker.Velocity[1] += (randFloat64() - 0.5) * 0.1 + walker.Velocity[0] += (rand.Float64() - 0.5) * 0.1 + walker.Velocity[1] += (rand.Float64() - 0.5) * 0.1 // Clamp velocity maxSpeed := 0.5 @@ -525,7 +525,7 @@ func (e *Engine) detectBlobs() []BlobResult { } // computeCSIAtPosition computes simulated CSI amplitude at a position. -func (e *Engine) computeCSIAtPosition(link *Link, pos [3]float64) float64 { +func (e *Engine) computeCSIAtPosition(link *EngineLink, pos [3]float64) float64 { // Simplified path loss model // PL(d) = PL_0 + 10*n*log10(d/d_0) // PL_0 = 40 dB at d_0 = 1m, n = 2.0 (free space) diff --git a/mothership/internal/simulator/physics.go b/mothership/internal/simulator/physics.go new file mode 100644 index 0000000..c4693f4 --- /dev/null +++ b/mothership/internal/simulator/physics.go @@ -0,0 +1,300 @@ +// Package simulator provides shared physics functions for CSI simulation. +// This package is used by both the pre-deployment simulator and the CSI CLI simulator. +package simulator + +import ( + "math" + "math/rand" +) + +// PhysicsModel provides physics calculations for CSI simulation +type PhysicsModel struct { + space *Space + noiseSigma float64 // Gaussian noise standard deviation for I/Q + walls []WallDefinition +} + +// WallDefinition defines a wall segment for attenuation calculations +type WallDefinition struct { + X1, Y1, X2, Y2 float64 // Wall endpoints (floor coordinates) + Attenuation float64 // dB attenuation +} + +// NewPhysicsModel creates a new physics model for the given space +func NewPhysicsModel(space *Space) *PhysicsModel { + return &PhysicsModel{ + space: space, + noiseSigma: 0.005, // Default noise level + walls: make([]WallDefinition, 0), + } +} + +// SetNoiseSigma sets the Gaussian noise standard deviation +func (pm *PhysicsModel) SetNoiseSigma(sigma float64) { + pm.noiseSigma = sigma +} + +// AddWall adds a wall definition to the physics model +func (pm *PhysicsModel) AddWall(x1, y1, x2, y2, attenuation float64) { + pm.walls = append(pm.walls, WallDefinition{ + X1: x1, + Y1: y1, + X2: x2, + Y2: y2, + Attenuation: attenuation, + }) +} + +// PathLossdB computes path loss in dB using log-distance model +// PL(d) = PL_0 + 10*n*log10(d/d_0) +// where PL_0 = 40 dB at d_0 = 1m, n = 2.0 (free space) +func (pm *PhysicsModel) PathLossdB(distance float64) float64 { + const PL0 = 40.0 // dB at 1m reference + const d0 = 1.0 // reference distance in meters + const n = 2.0 // path loss exponent (free space) + + if distance < 0.01 { + distance = 0.01 // Avoid log(0) + } + + return PL0 + 10*n*math.Log10(distance/d0) +} + +// WallAttenuation computes total wall attenuation for a path +func (pm *PhysicsModel) WallAttenuation(from, to Point) float64 { + totalLoss := 0.0 + + for _, wall := range pm.walls { + if pm.pathIntersectsWall(from.X, from.Y, to.X, to.Y, + wall.X1, wall.Y1, wall.X2, wall.Y2) { + totalLoss += wall.Attenuation + } + } + + return totalLoss +} + +// pathIntersectsWall checks if a path intersects a wall segment (2D) +func (pm *PhysicsModel) pathIntersectsWall(x1, y1, x2, y2, wx1, wy1, wx2, wy2 float64) bool { + // Compute orientations + ccw := func(ax, ay, bx, by, cx, cy float64) float64 { + return (bx-ax)*(cy-ay) - (by-ay)*(cx-ax) + } + + o1 := ccw(x1, y1, x2, y2, wx1, wy1) + o2 := ccw(x1, y1, x2, y2, wx2, wy2) + o3 := ccw(wx1, wy1, wx2, wy2, x1, y1) + o4 := ccw(wx1, wy1, wx2, wy2, x2, y2) + + // Check for intersection + return o1*o2 < 0 && o3*o4 < 0 +} + +// ComputeRSSI computes the RSSI in dBm for a given distance +// Returns RSSI in range [-90, -30] dBm +func (pm *PhysicsModel) ComputeRSSI(distance float64) int8 { + pathLoss := pm.PathLossdB(distance) + txPower := -30.0 // Reference transmit power in dBm + + rssi := txPower - pathLoss + + // Clamp to realistic range + if rssi < -90 { + rssi = -90 + } + if rssi > -30 { + rssi = -30 + } + + return int8(rssi) +} + +// DeltaRMS computes the expected deltaRMS motion score +// when a walker is at the given position (vs empty room) +func (pm *PhysicsModel) DeltaRMS(tx, rx, walker Point) float64 { + // Calculate Fresnel zone number + zone := FresnelZoneNumber(tx, rx, walker) + + // DeltaRMS is highest in zone 1, decreases with zone number + // Zone 1: 0.15, Zone 2: 0.08, Zone 3: 0.04, Zone 4: 0.02, Zone 5+: 0.01 + switch zone { + case 1: + return 0.15 + case 2: + return 0.08 + case 3: + return 0.04 + case 4: + return 0.02 + default: + return 0.01 + } +} + +// GenerateIQPair generates a synthetic I/Q pair for a subcarrier +// with amplitude and phase, plus Gaussian noise +func (pm *PhysicsModel) GenerateIQPair(amplitude, phase float64) (int8, int8) { + // Generate Gaussian noise using Box-Muller transform + u1 := rand.Float64() + u2 := rand.Float64() + z0 := math.Sqrt(-2.0*math.Log(u1)) * math.Cos(2.0*math.Pi*u2) + z1 := math.Sqrt(-2.0*math.Log(u1)) * math.Sin(2.0*math.Pi*u2) + + noiseI := z0 * pm.noiseSigma + noiseQ := z1 * pm.noiseSigma + + // Convert to I/Q + i := amplitude*math.Cos(phase) + noiseI + q := amplitude*math.Sin(phase) + noiseQ + + // Clamp to int8 range [-127, 127] + // Note: We avoid -128 to prevent overflow issues + if i > 127 { + i = 127 + } + if i < -127 { + i = -127 + } + if q > 127 { + q = 127 + } + if q < -127 { + q = -127 + } + + return int8(i), int8(q) +} + +// GenerateSubcarrierCSI generates CSI data for all subcarriers +func (pm *PhysicsModel) GenerateSubcarrierCSI(tx, rx, walker Point, nSub int, frameNum int) []struct{ I, Q int8 } { + result := make([]struct{ I, Q int8 }, nSub) + + // Base amplitude from deltaRMS + deltaRMS := pm.DeltaRMS(tx, rx, walker) + amplitude := deltaRMS * 500.0 // Scale to reasonable I/Q range + + for k := 0; k < nSub; k++ { + // Compute phase at this subcarrier + phase := pm.phaseAtSubcarrier(tx, rx, walker, k, frameNum) + + // Add subcarrier-dependent amplitude variation + // Simulates frequency-selective fading + freqFading := 0.8 + 0.4*math.Sin(2*math.Pi*float64(k)/16.0) + subAmplitude := amplitude * freqFading + + result[k].I, result[k].Q = pm.GenerateIQPair(subAmplitude, phase) + } + + return result +} + +// PhaseAtSubcarrier computes phase for a given subcarrier index +func (pm *PhysicsModel) PhaseAtSubcarrier(tx, rx, walker Point, subcarrierIndex, frameNum int) float64 { + // Total path length (TX -> walker -> RX) + d1 := tx.Distance(walker) + d2 := walker.Distance(rx) + totalDist := d1 + d2 + + // Phase = 2π × k × Δf × (d / c) + temporal_variation + phase := 2*math.Pi*float64(subcarrierIndex)*SubcarrierSpacing*(totalDist/C) + + // Add small temporal variation for realism + temporalPhase := 0.1 * math.Sin(2*math.Pi*float64(frameNum)/100.0) + phase += temporalPhase + + // Normalize to [-π, π] + for phase > math.Pi { + phase -= 2 * math.Pi + } + for phase < -math.Pi { + phase += 2 * math.Pi + } + + return phase +} + +// ValidateRSSI validates that RSSI is within expected range for distance +func ValidateRSSI(rssi int8, distance float64) bool { + // Expected RSSI range for given distance + expectedPathLoss := 40.0 + 20.0*math.Log10(distance/1.0) + expectedRSSI := -30.0 - expectedPathLoss + + // Allow ±20 dB tolerance + minRSSI := expectedRSSI - 20.0 + maxRSSI := expectedRSSI + 20.0 + + // Clamp to realistic bounds + if minRSSI < -90 { + minRSSI = -90 + } + if maxRSSI > -30 { + maxRSSI = -30 + } + + return float64(rssi) >= minRSSI && float64(rssi) <= maxRSSI +} + +// ValidateIQValues checks that I/Q values are in valid int8 range +func ValidateIQValues(i, q int8) bool { + return i >= -127 && i <= 127 && q >= -127 && q <= 127 +} + +// IsInFresnelZones checks if a point is within the first N Fresnel zones +func IsInFresnelZones(tx, rx, point Point, maxZone int) bool { + zone := FresnelZoneNumber(tx, rx, point) + return zone <= maxZone && zone > 0 +} + +// ComputeFresnelModulation computes the Fresnel zone modulation factor +// Returns a value between 0 and 1, where 1 is maximum modulation (zone 1) +func ComputeFresnelModulation(tx, rx, point Point) float64 { + zone := FresnelZoneNumber(tx, rx, point) + + // Zone 1: maximum modulation, Zone 5+: minimum + if zone <= 1 { + return 1.0 + } + if zone >= 5 { + return 0.0 + } + + return 1.0 / math.Pow(float64(zone), 2.0) +} + +// ComputeLinkQuality estimates link quality (0-1) based on geometry +// Higher quality when links have good angular diversity +func ComputeLinkQuality(nodes []Point) float64 { + if len(nodes) < 2 { + return 0.0 + } + + // Simple metric: spread of node positions + // Compute centroid + var cx, cy, cz float64 + for _, n := range nodes { + cx += n.X + cy += n.Y + cz += n.Z + } + cx /= float64(len(nodes)) + cy /= float64(len(nodes)) + cz /= float64(len(nodes)) + + // Compute average distance from centroid + avgDist := 0.0 + for _, n := range nodes { + dx := n.X - cx + dy := n.Y - cy + dz := n.Z - cz + avgDist += math.Sqrt(dx*dx + dy*dy + dz*dz) + } + avgDist /= float64(len(nodes)) + + // Normalize: 5m spread = excellent quality (1.0) + quality := avgDist / 5.0 + if quality > 1.0 { + quality = 1.0 + } + + return quality +} diff --git a/mothership/internal/simulator/propagation.go b/mothership/internal/simulator/propagation.go index 8aeaccd..0089d79 100644 --- a/mothership/internal/simulator/propagation.go +++ b/mothership/internal/simulator/propagation.go @@ -489,7 +489,3 @@ func IsInFirstFresnelZone(tx, rx, point Point) bool { return FresnelZoneNumber(tx, rx, point) == 1 } -// IsInFresnelZones returns true if the point is within the first N Fresnel zones -func IsInFresnelZones(tx, rx, point Point, maxZone int) bool { - return FresnelZoneNumber(tx, rx, point) <= maxZone -}