package main

import (
    "fmt"
    "math"
    "sync"
)

type Coord3D struct { X, Y, Z float64 }

type Patch struct {
    ID        uint64
    Position  Coord3D
    Size      Coord3D
    FeatureVec []float32
    Timestamp int64
    Confidence float64
}

type OctreeNode struct {
    Center    Coord3D
    HalfSize  Coord3D
    Children  [8]*OctreeNode
    Patches   []uint64
    IsLeaf    bool
    Capacity  int
}

type SpatialMemory struct {
    mu       sync.RWMutex
    patches  map[uint64]*Patch
    octree   *OctreeNode
}

func NewSpatialMemory() *SpatialMemory {
    return &SpatialMemory{
        patches: make(map[uint64]*Patch),
        octree: &OctreeNode{
            Center: Coord3D{0, 0, 0}, HalfSize: Coord3D{100, 100, 100},
            IsLeaf: true, Capacity: 16,
        },
    }
}

func (sm *SpatialMemory) QueryRange(center Coord3D, radius float64) []*Patch {
    sm.mu.RLock()
    defer sm.mu.RUnlock()
    
    result := make([]*Patch, 0)
    // Octree range query
    var query func(node *OctreeNode)
    query = func(node *OctreeNode) {
        dx := math.Max(0, math.Abs(center.X-node.Center.X)-node.HalfSize.X)
        dy := math.Max(0, math.Abs(center.Y-node.Center.Y)-node.HalfSize.Y)
        dz := math.Max(0, math.Abs(center.Z-node.Center.Z)-node.HalfSize.Z)
        if dx*dx+dy*dy+dz*dz > radius*radius { return }
        
        if node.IsLeaf {
            for _, id := range node.Patches {
                if p, ok := sm.patches[id]; ok {
                    result = append(result, p)
                }
            }
            return
        }
        for _, child := range node.Children {
            if child != nil { query(child) }
        }
    }
    query(sm.octree)
    return result
}

import torch
import torch.nn as nn
import numpy as np

class PatchMemory(nn.Module):
    """Patch-level 3D spatial memory module"""
    
    def __init__(self, feature_dim=128, capacity=100000):
        super().__init__()
        self.feature_dim = feature_dim
        self.memory_bank = []
        self.encoder = nn.Sequential(
            nn.Conv2d(3, 64, 3, padding=1),
            nn.BatchNorm2d(64), nn.ReLU(),
            nn.Conv2d(64, 128, 3, stride=2, padding=1),
            nn.BatchNorm2d(128), nn.ReLU(),
            nn.Conv2d(128, feature_dim, 3, stride=2, padding=1),
            nn.AdaptiveAvgPool2d(1),
        )
    
    def encode_frame(self, frame):
        B, C, H, W = frame.shape
        patch_size = 16
        patches = []
        
        for i in range(0, H, patch_size):
            for j in range(0, W, patch_size):
                patch = frame[:, :, i:i+patch_size, j:j+patch_size]
                if patch.shape[2] < patch_size: continue
                feat = self.encoder(patch)
                patches.append(feat.squeeze(-1).squeeze(-1))
        
        return torch.stack(patches, dim=1)
    
    def forward(self, query_feat, query_pos):
        # Simplified memory fusion
        nearby = self.memory_bank[:32] if self.memory_bank else []
        if not nearby:
            return query_feat
        
        memory_feats = torch.stack([f for f in nearby[:32]])
        fused = torch.cat([query_feat.unsqueeze(0), memory_feats], dim=0)
        return fused.mean(dim=0)  # Average fusion

import numpy as np

class PhysicsEngine:
    """Built-in physics simulation"""
    
    def __init__(self, gravity=9.81):
        self.gravity = np.array([0, -gravity, 0])
        self.objects = []
    
    def step(self, dt):
        # Apply gravity
        for obj in self.objects:
            if not obj.get("static", False):
                obj["velocity"] += self.gravity * dt
                obj["position"] += obj["velocity"] * dt
        
        # Collision detection
        for i in range(len(self.objects)):
            for j in range(i+1, len(self.objects)):
                a, b = self.objects[i], self.objects[j]
                dist = np.linalg.norm(a["position"] - b["position"])
                if dist < 2.0:  # Collision threshold
                    self._resolve_collision(a, b, dist)
    
    def _resolve_collision(self, a, b, dist):
        normal = (b["position"] - a["position"]) / dist
        rel_vel = a["velocity"] - b["velocity"]
        vel_along = np.dot(rel_vel, normal)
        if vel_along > 0: return
        
        e = min(a.get("restitution", 0.5), b.get("restitution", 0.5))
        j_mag = -(1 + e) * vel_along / (1/a["mass"] + 1/b["mass"])
        impulse = j_mag * normal
        a["velocity"] += impulse / a["mass"]
        b["velocity"] -= impulse / b["mass"]