Reorganize project structure and create development roadmap

- Move documentation to organized docs/ directory structure
- Add dev notes
- Create comprehensive 5-phase roadmap for indoor positioning system
- Move AT command manual and hardware images to docs/
- Update README with hardware links and project overview
- Remove sleep mode and OTA functionality for simplification
- Clean up project structure for production development

CLAUDE.md

@@ -0,0 +1,106 @@
+# CLAUDE.md
+
+This file provides guidance to Claude Code (claude.ai/code) when working with code in this repository.
+
+## Project Overview
+
+This is an ESP32-S3 based Ultra-Wideband (UWB) indoor positioning system for warehouse WiFi mapping. The system consists of anchor devices (base stations) and tag devices (mobile trackers) using Makerfabs MaUWB modules with Qorvo DW3000 chips.
+
+**Key Architecture**: Battery-powered anchors auto-position themselves, mobile tag collects positioning data via USB to PC for real-time display and dual-file logging for offline analysis.
+
+## Development Commands
+
+### Build & Flash
+```bash
+# Build specific device type
+pio run -e anchor      # Build anchor firmware
+pio run -e tag         # Build tag firmware  
+pio run -e tag2        # Build second tag (ID 2)
+
+# Flash to device
+pio run -e tag -t upload
+pio run -e anchor -t upload
+
+# Monitor serial output
+pio device monitor
+```
+
+### Development Environments
+- **anchor**: Base station (UWB_INDEX=0, tracks tags)
+- **tag**: Mobile tracker (UWB_INDEX=1, reports to anchors)
+- **tag2**: Second mobile tracker (UWB_INDEX=2)
+- **debug**: Development build with DEBUG_ENABLED=1
+
+## Core Architecture
+
+### Device Roles & Communication Flow
+```
+Anchors: Auto-position ’ Store coordinates locally ’ Report to tags
+Tags: Range to anchors ’ Collect coordinates ’ USB to PC ’ Real-time + logging
+```
+
+### Key Technical Patterns
+
+**1. Dual Firmware Architecture**
+- `main_anchor.cpp`: Tracks tags, manages UWB_TAG_COUNT devices, displays active tags
+- `main_tag.cpp`: Connects to MAX_ANCHORS, reports to PC via USB, displays anchor distances
+- Shared: config.h for hardware pins, UWBHelper library for AT commands
+
+**2. UWBHelper Library Structure**
+- **Complete AT Command Implementation**: All 21 commands from AT manual
+- **Device Role Configuration**: `configureDevice(id, isAnchor)` sets anchor/tag mode
+- **Advanced Data Structures**: RangeResult, AnchorPosition, DeviceData for multi-device tracking
+- **Position Calculation**: PositionCalculator class with trilateration algorithms
+- **Data Filtering**: DistanceFilter for noise reduction
+
+**3. Hardware Configuration**
+- **UWB Communication**: UART2 (pins 17/18) at 115200 baud
+- **Display**: SSD1306 OLED via I2C (pins 38/39)
+- **Reset**: Pin 16 for UWB module reset
+- **Network**: ID 1234, 6.8Mbps mode, range filtering enabled
+
+### Critical Implementation Details
+
+**Device Identification**: Each device gets unique UWB_INDEX via build flags, determines network role and behavior
+
+**Data Flow Pattern**: 
+- Anchors parse range data from tags: `parseRangeData(response, tags[], UWB_TAG_COUNT)`
+- Tags parse range data from anchors: `parseRangeData(response, anchors[], MAX_ANCHORS)`
+- Serial passthrough enabled for debugging: `uwbSerial.write(Serial.read())`
+
+**Display Logic**: 
+- Anchors show active tags with distances/RSSI
+- Tags show connected anchors with distances/RSSI  
+- Both show network status and device counts
+
+**Timeout Management**: Devices marked inactive after DEVICE_TIMEOUT (5000ms) with no updates
+
+## Configuration Constants
+
+Key values in config.h:
+- `MAX_ANCHORS 8`: Tag connects to 8 closest anchors
+- `UWB_TAG_COUNT 64`: Network supports up to 64 tags
+- `NETWORK_ID 1234`: UWB network identifier
+- `DEVICE_TIMEOUT 5000`: Device inactivity timeout (ms)
+- `DISPLAY_UPDATE_INTERVAL 500`: OLED refresh rate (ms)
+
+## Development Roadmap Context
+
+This system implements **Phase 1** of a 5-phase indoor positioning project (see docs/ROADMAP.md):
+- **Current**: Basic anchor-tag ranging with USB data collection
+- **Next**: Anchor auto-positioning, dual-file logging, PC software, web visualization
+- **Goal**: <15min warehouse setup, <30cm accuracy, real-time + offline analysis
+
+## Hardware Requirements
+
+- **Modules**: Makerfabs MaUWB ESP32-S3 with built-in UWB + OLED
+- **Power**: USB for development, battery for production anchors
+- **Range**: Tested up to 100m line-of-sight, 30m through walls
+- **Network**: No WiFi required, pure UWB communication
+
+## Important Notes
+
+- **No Sleep/OTA**: Removed for simplicity, focus on core positioning
+- **AT Command Protocol**: Direct UART communication with UWB module
+- **Position Calculation**: Client-side (PC) processing, not on-device
+- **Data Logging**: Raw distance data + anchor coordinates for offline analysis

README.md

@@ -1,6 +1,8 @@
 # MaUWB ESP32-S3 Positioning System
 
-Ultra-wideband (UWB) positioning system using ESP32-S3 and Makerfabs UWB modules.
+Ultra-wideband (UWB) positioning system using ESP32-S3 and Makerfabs UWB modules for indoor positioning and warehouse mapping applications.
+
+![Working MaUWB Devices](docs/images/20250819_202527.jpg)
 
 ## Features
 - ESP32-S3 based anchor and tag devices
@@ -8,12 +10,14 @@ Ultra-wideband (UWB) positioning system using ESP32-S3 and Makerfabs UWB modules
 - OLED display for status and measurements
 - Multiple tag support (up to 64 tags)
 - 6.8Mbps communication rate
-- Serial debugging output
+- Complete AT command implementation
+- Position calculation with trilateration
+- Anchor auto-positioning system
+- Real-time positioning with USB data logging
 
-## Hardware
+![MaUWB Modules](docs/images/20250819_202629.jpg)
-- ESP32-S3 DevKit
+
-- Makerfabs UWB AT Module
+**Hardware:** [Makerfabs MaUWB ESP32-S3 UWB Module](https://www.makerfabs.com/mauwb-esp32s3-uwb-module.html) with SSD1306 OLED displays
-- SSD1306 OLED Display (128x64)
 
 ## Environments
 - `anchor`: Base station for positioning
@@ -33,8 +37,63 @@ pio run -e tag -t upload
 pio device monitor
 ```
 
+## AT Command Support
+
+Complete implementation of all AT commands from the official manual:
+
+### Basic Commands
+- `AT?` - Test connection
+- `AT+GETVER?` - Get firmware version
+- `AT+RESTART` - Restart module
+- `AT+RESTORE` - Factory reset
+- `AT+SAVE` - Save configuration
+
+### Configuration
+- `AT+SETCFG` / `AT+GETCFG?` - Device configuration
+- `AT+SETANT` / `AT+GETANT?` - Antenna delay calibration
+- `AT+SETCAP` / `AT+GETCAP?` - System capacity settings
+- `AT+SETRPT` / `AT+GETRPT?` - Auto-reporting control
+
+### Network & Power
+- `AT+SETPAN` / `AT+GETPAN?` - Network ID configuration
+- `AT+SETPOW` / `AT+GETPOW?` - Transmission power control
+- `AT+SLEEP` - Sleep mode for battery conservation
+
+### Data Communication
+- `AT+DATA` / `AT+RDATA` - Custom data transmission
+- Real-time range reporting via `AT+RANGE` parsing
+
+## Library Features
+
+The enhanced UWBHelper library provides:
+- **Complete AT command coverage**
+- **Advanced range data parsing** for multiple anchors
+- **Position calculation algorithms** (trilateration, multilateration)
+- **Anchor position management** for auto-positioning
+- **Distance filtering** for improved accuracy
+- **Backward compatibility** with existing code
+
 ## Configuration
-- Network ID: 1234
+- **Network ID**: 1234 (configurable via AT+SETPAN)
-- Baud Rate: 115200
+- **Baud Rate**: 115200
-- Communication: 6.8Mbps
+- **Communication**: 6.8Mbps (AT+SETCFG parameter)
-- Range filtering: Enabled
+- **Range filtering**: Enabled for accuracy
+- **Refresh Rate**: 10Hz (configurable via AT+SETCAP)
+- **Max Anchors**: Unlimited (tags connect to 8 closest)
+- **Max Tags**: 64 per network
+
+
+## Documentation
+
+- [AT Command Manual](docs/manuals/Makerfabs%20UWB%20AT%20Module%20AT%20Command%20Manual(v1.1.1).pdf) - Complete AT command reference
+- [Hardware Product Page](https://www.makerfabs.com/mauwb-esp32s3-uwb-module.html) - Official hardware documentation
+- [Project Roadmap](docs/ROADMAP.md) - Development plan for indoor positioning system
+
+## Applications
+
+This system is designed for:
+- **Indoor positioning** in warehouses and large buildings
+- **Asset tracking** and inventory management
+- **Navigation assistance** in GPS-denied environments
+- **WiFi signal mapping** and coverage analysis
+- **Research and development** in UWB positioning

docs/ROADMAP.md

@@ -0,0 +1,255 @@
+# Indoor Positioning System Roadmap
+**MaUWB ESP32-S3 Warehouse Positioning & WiFi Mapping System**
+
+---
+
+## 🎯 Project Overview
+
+**Purpose**: Indoor positioning system for warehouse WiFi strength mapping
+- **Hardware**: 1 mobile tag + 8+ battery-powered anchors
+- **Connectivity**: Tag connects to PC via USB, anchors are wireless-only
+- **Constraint**: Tag connects to 8 closest anchors (auto-switching)
+- **Goal**: Quick installation (<15 min) with automatic anchor positioning
+
+---
+
+## 📋 System Requirements
+
+### Hardware Components
+- [ ] **Tag Device**: ESP32-S3 with USB connectivity
+- [ ] **Anchor Devices**: 8+ ESP32-S3 units (battery powered)
+- [ ] **PC Interface**: USB connection for data collection
+- [ ] **Power Management**: Battery-powered anchors (no mains/WiFi)
+
+### Core Constraints
+- ✅ Battery-powered anchors (no WiFi connectivity)
+- ✅ Tag connects to 8 closest anchors (auto-switching)
+- ✅ USB-only connection (tag to PC)
+- ✅ No WiFi scanning needed (handled by PC)
+- ✅ Warehouse too large for simultaneous connection to all anchors
+
+---
+
+## 🏗️ System Architecture
+
+### Data Flow Design
+```
+Anchors (Battery) → Auto-Position Calibration → Store Coordinates Locally
+                                                        ↓
+Tag (Mobile) → Collect Raw Data + Anchor Coordinates → PC (USB) → Real-time Display
+                                    ↓                                      ↓
+                              Two Log Files                        Live Position View
+                                    ↓
+                            Web Application
+                         (Load both files)
+```
+
+### Critical Problem Solved
+**Challenge**: How do battery-powered anchors transmit calibrated positions to PC?
+**Solution**: Dual-file approach - Tag logs raw positioning data + anchor coordinates separately, webapp loads both files for offline processing
+
+---
+
+## 🚀 Implementation Roadmap
+
+### Phase 1: Anchor Auto-Positioning System
+**Duration**: 2-3 weeks
+
+#### 1.1 Distributed Positioning Algorithm
+- [ ] **Anchor Discovery Protocol**
+  - All anchors broadcast discovery signals on startup
+  - Build neighbor discovery table for each anchor
+  - Implement range-based network topology mapping
+
+- [ ] **Distance Measurement Matrix**
+  - Each anchor measures distances to all neighbors in range
+  - Store distance measurements locally (EEPROM/flash)
+  - Handle partial connectivity (not all anchors can reach each other)
+
+- [ ] **Coordinate System Establishment**
+  - Designate anchor with most connections as origin (0,0)
+  - Establish coordinate system orientation
+  - Implement distributed position calculation algorithm
+
+- [ ] **Position Calculation & Storage**
+  - Each anchor calculates its own position using trilateration
+  - Store calculated position in local memory
+  - Implement position confidence scoring
+
+#### 1.2 Anchor Communication Protocol
+- [ ] **Inter-Anchor Data Exchange**
+  - Protocol for sharing distance measurements
+  - Handle multi-hop communication for distant anchors
+  - Implement data consistency checks
+
+- [ ] **Position Refinement**
+  - Iterative position improvement algorithm
+  - Consensus mechanism for coordinate system alignment
+  - Error detection and correction
+
+### Phase 2: Tag Data Relay System
+**Duration**: 2 weeks
+
+#### 2.1 Enhanced Tag Functionality
+- [ ] **Anchor Discovery & Connection**
+  - Scan for available anchors
+  - Connect to 8 closest/strongest anchors
+  - Implement smooth anchor switching logic
+
+- [ ] **Position Data Collection**
+  - Request calibrated positions from connected anchors
+  - Aggregate anchor position data
+  - Handle missing or incomplete anchor data
+
+- [ ] **Real-time Positioning**
+  - Calculate tag position using 8 connected anchors
+  - Maintain position continuity during anchor handoffs
+  - Implement position smoothing/filtering
+
+#### 2.2 Tag Data Logging System
+- [ ] **Dual-File Logging**
+  - **File 1**: Raw positioning data (distances, RSSI, timestamps)
+  - **File 2**: Anchor coordinates database (collected from connected anchors)
+  - USB transfer to PC for both files
+
+- [ ] **Data Collection Protocol**
+  - Request anchor coordinates: "Send me your calibrated position"
+  - Anchor responds: {anchor_id, x, y, calibration_confidence}
+  - Store coordinates locally and update anchor database file
+  - Continue logging raw positioning data as current system does
+
+### Phase 3: PC Software Development
+**Duration**: 2 weeks
+
+#### 3.1 Real-time PC Application
+- [ ] **USB Communication & Live Display**
+  - Receive real-time data stream from tag via USB
+  - Parse incoming data: raw distances + anchor coordinates
+  - Calculate live tag position using anchor coordinates
+  - Display real-time position on 2D map
+
+- [ ] **Live Monitoring Features**
+  - Show current tag position with live updates
+  - Display connected anchors and their positions
+  - Real-time signal strength indicators
+  - Live path tracking during mapping session
+
+#### 3.2 Dual-File Logging (Background)
+- [ ] **Simultaneous Data Logging**
+  - **raw_positioning.csv**: Tag positioning data (distances, RSSI, timestamps)
+  - **anchor_coordinates.csv**: Anchor position database
+  - Log files generated automatically during real-time session
+  - Export files for webapp analysis after session
+
+- [ ] **Data Validation**
+  - Verify file integrity and format
+  - Check timestamp consistency
+  - Validate anchor coordinate data
+
+### Phase 4: Web Visualization Application
+**Duration**: 2-3 weeks
+
+#### 4.1 Core Web Interface
+- [ ] **Dual-File Upload & Processing**
+  - Upload **raw_positioning.csv** and **anchor_coordinates.csv**
+  - Parse and correlate both datasets
+  - Data validation and error handling
+  - Calculate actual tag positions using raw data + anchor coordinates
+
+- [ ] **2D Warehouse Visualization**
+  - Display anchor positions from coordinates file
+  - Calculate and plot tag path using positioning algorithm
+  - Interactive warehouse floor plan with scalable coordinate system
+
+#### 4.2 Path Analysis Features
+- [ ] **Path Tracking Visualization**
+  - Tag movement path overlay
+  - Timeline scrubbing and playback
+  - Speed and direction indicators
+
+- [ ] **Data Analysis Tools**
+  - Path statistics and metrics
+  - Export functionality (images, reports)
+  - Comparison between multiple mapping sessions
+
+### Phase 5: System Integration & Optimization
+**Duration**: 1-2 weeks
+
+#### 5.1 Quick Installation Workflow
+- [ ] **Automated Setup Process**
+  - Power-on all anchors simultaneously
+  - Auto-discovery and network formation
+  - Position calibration and verification
+  - Tag pairing and PC connection setup
+  - Target: Ready-to-use in <15 minutes
+
+#### 5.2 System Validation
+- [ ] **Accuracy Testing**
+  - Position accuracy validation
+  - Anchor auto-positioning verification
+  - End-to-end system testing
+
+- [ ] **Performance Optimization**
+  - Battery life optimization for anchors
+  - Data transmission efficiency
+  - Real-time performance tuning
+
+---
+
+## 🎯 Key Technical Challenges
+
+### 1. Distributed Anchor Positioning
+**Challenge**: Anchors must calculate positions without central coordination
+**Solution**: Implement distributed trilateration with consensus mechanism
+
+### 2. Data Relay Through Tag
+**Challenge**: Getting anchor position data to PC without direct connectivity
+**Solution**: Tag acts as mobile bridge collecting and relaying data
+
+### 3. Coordinate System Consistency
+**Challenge**: Ensuring all anchors use same coordinate system
+**Solution**: Distributed coordinate system establishment protocol
+
+### 4. Anchor Handoff Management
+**Challenge**: Smooth positioning during anchor switching
+**Solution**: Position continuity algorithms and coordinate system alignment
+
+### 5. Partial Connectivity Handling
+**Challenge**: Not all anchors can communicate directly
+**Solution**: Multi-hop communication and distributed data sharing
+
+---
+
+## 📦 Deliverables
+
+### Software Components
+- [ ] **Enhanced Anchor Firmware** - Auto-positioning and data storage
+- [ ] **Enhanced Tag Firmware** - Data relay and USB communication
+- [ ] **PC Data Collection Software** - USB interface and logging
+- [ ] **Web Visualization Application** - Path analysis and mapping
+
+### Documentation
+- [ ] **Installation Guide** - Quick setup procedures
+- [ ] **User Manual** - Operation and troubleshooting
+- [ ] **Technical Documentation** - API and protocol specifications
+- [ ] **Calibration Procedures** - System validation and accuracy testing
+
+### Test Results
+- [ ] **Positioning Accuracy Report** - Performance metrics
+- [ ] **Battery Life Analysis** - Power consumption data
+- [ ] **Installation Time Study** - Setup procedure validation
+
+---
+
+## 🔄 Success Criteria
+
+1. **Installation Time**: Complete system setup in <15 minutes
+2. **Positioning Accuracy**: <30cm accuracy in warehouse environment
+3. **Battery Life**: Anchors operate >8 hours on single charge
+4. **System Reliability**: 99%+ uptime during mapping sessions
+5. **Data Integrity**: Complete path tracking with <1% data loss
+6. **User Experience**: Simple web interface for path visualization
+
+
+*Last Updated: 2025-01-19*
+*Version: 1.0*

lib/UWBHelper/UWBHelper.cpp

@@ -1,32 +1,42 @@
 #include "UWBHelper.h"
+#include <math.h>
 
 UWBHelper::UWBHelper(HardwareSerial* serial, int reset) {
     uwbSerial = serial;
     resetPin = reset;
 }
 
-bool UWBHelper::begin() {
+// ===== BASIC COMMANDS (3.2-3.6) =====
-    pinMode(resetPin, OUTPUT);
+
-    digitalWrite(resetPin, HIGH);
+bool UWBHelper::testConnection() {
-    
-    uwbSerial->begin(115200);
-    delay(1000);
-    
-    // Test communication
     String response = sendCommand("AT?", 2000);
     return isResponseOK(response);
 }
 
-void UWBHelper::reset() {
+String UWBHelper::getVersion() {
-    digitalWrite(resetPin, LOW);
+    return sendCommand("AT+GETVER?", 2000);
-    delay(100);
-    digitalWrite(resetPin, HIGH);
-    delay(1000);
 }
 
-bool UWBHelper::configureDevice(int deviceId, bool isAnchor, int dataRate, int rangeFilter) {
+bool UWBHelper::restart() {
+    String response = sendCommand("AT+RESTART", 2000);
+    return isResponseOK(response);
+}
+
+bool UWBHelper::restore() {
+    String response = sendCommand("AT+RESTORE", 2000);
+    return isResponseOK(response);
+}
+
+bool UWBHelper::save() {
+    String response = sendCommand("AT+SAVE", 2000);
+    return isResponseOK(response);
+}
+
+// ===== CONFIGURATION COMMANDS (3.7-3.8) =====
+
+bool UWBHelper::setConfig(int deviceId, int role, int dataRate, int rangeFilter) {
     String cmd = "AT+SETCFG=" + String(deviceId) + "," + 
-                 String(isAnchor ? 1 : 0) + "," + 
+                 String(role) + "," + 
                  String(dataRate) + "," + 
                  String(rangeFilter);
     
@@ -34,6 +44,24 @@ bool UWBHelper::configureDevice(int deviceId, bool isAnchor, int dataRate, int r
     return isResponseOK(response);
 }
 
+String UWBHelper::getConfig() {
+    return sendCommand("AT+GETCFG?", 2000);
+}
+
+// ===== ANTENNA COMMANDS (3.9-3.10) =====
+
+bool UWBHelper::setAntennaDelay(int delay) {
+    String cmd = "AT+SETANT=" + String(delay);
+    String response = sendCommand(cmd, 2000);
+    return isResponseOK(response);
+}
+
+String UWBHelper::getAntennaDelay() {
+    return sendCommand("AT+GETANT?", 2000);
+}
+
+// ===== CAPACITY COMMANDS (3.11-3.12) =====
+
 bool UWBHelper::setCapacity(int tagCount, int timeSlot, int extMode) {
     String cmd = "AT+SETCAP=" + String(tagCount) + "," + 
                  String(timeSlot) + "," + 
@@ -43,28 +71,104 @@ bool UWBHelper::setCapacity(int tagCount, int timeSlot, int extMode) {
     return isResponseOK(response);
 }
 
+String UWBHelper::getCapacity() {
+    return sendCommand("AT+GETCAP?", 2000);
+}
+
+// ===== REPORTING COMMANDS (3.13-3.14) =====
+
+bool UWBHelper::setReporting(bool enable) {
+    String cmd = "AT+SETRPT=" + String(enable ? 1 : 0);
+    String response = sendCommand(cmd, 2000);
+    return isResponseOK(response);
+}
+
+String UWBHelper::getReporting() {
+    return sendCommand("AT+GETRPT?", 2000);
+}
+
+// ===== SLEEP COMMAND (3.16) =====
+
+bool UWBHelper::setSleep(int sleepTime) {
+    String cmd = "AT+SLEEP=" + String(sleepTime);
+    String response = sendCommand(cmd, 2000);
+    return isResponseOK(response);
+}
+
+// ===== POWER COMMANDS (3.17-3.18) =====
+
+bool UWBHelper::setPower(String powerValue) {
+    String cmd = "AT+SETPOW=" + powerValue;
+    String response = sendCommand(cmd, 2000);
+    return isResponseOK(response);
+}
+
+String UWBHelper::getPower() {
+    return sendCommand("AT+GETPOW?", 2000);
+}
+
+// ===== DATA COMMANDS (3.19-3.20) =====
+
+bool UWBHelper::sendData(int length, String data) {
+    String cmd = "AT+DATA=" + String(length) + "," + data;
+    String response = sendCommand(cmd, 2000);
+    return isResponseOK(response);
+}
+
+String UWBHelper::receiveData() {
+    return sendCommand("AT+RDATA", 2000);
+}
+
+// ===== NETWORK COMMANDS (3.21-3.22) =====
+
 bool UWBHelper::setNetwork(int networkId) {
     String cmd = "AT+SETPAN=" + String(networkId);
     String response = sendCommand(cmd, 2000);
     return isResponseOK(response);
 }
 
+String UWBHelper::getNetwork() {
+    return sendCommand("AT+GETPAN?", 2000);
+}
+
+// ===== LEGACY WRAPPER FUNCTIONS =====
+
+bool UWBHelper::begin() {
+    pinMode(resetPin, OUTPUT);
+    digitalWrite(resetPin, HIGH);
+    
+    uwbSerial->begin(115200);
+    delay(1000);
+    
+    // Test communication
+    return testConnection();
+}
+
+void UWBHelper::reset() {
+    digitalWrite(resetPin, LOW);
+    delay(100);
+    digitalWrite(resetPin, HIGH);
+    delay(1000);
+}
+
+bool UWBHelper::configureDevice(int deviceId, bool isAnchor, int dataRate, int rangeFilter) {
+    return setConfig(deviceId, isAnchor ? 1 : 0, dataRate, rangeFilter);
+}
+
 bool UWBHelper::enableReporting(bool enable) {
-    String cmd = "AT+SETRPT=" + String(enable ? 1 : 0);
+    return setReporting(enable);
-    String response = sendCommand(cmd, 2000);
-    return isResponseOK(response);
 }
 
 bool UWBHelper::saveConfiguration() {
-    String response = sendCommand("AT+SAVE", 2000);
+    return save();
-    return isResponseOK(response);
 }
 
 bool UWBHelper::restartDevice() {
-    String response = sendCommand("AT+RESTART", 2000);
+    return restart();
-    return isResponseOK(response);
 }
 
+// ===== COMMUNICATION =====
+
 String UWBHelper::sendCommand(String command, int timeout) {
     String response = "";
     
@@ -75,8 +179,16 @@ String UWBHelper::sendCommand(String command, int timeout) {
     while ((millis() - startTime) < timeout) {
         while (uwbSerial->available()) {
             char c = uwbSerial->read();
-            response += c;
+            if (c == '\n' || c == '\r') {
+                if (response.length() > 0) {
+                    Serial.println("Response: " + response);
+                    return response;
+                }
+            } else {
+                response += c;
+            }
         }
+        delay(1);
     }
     
     if (response.length() > 0) {
@@ -85,6 +197,8 @@ String UWBHelper::sendCommand(String command, int timeout) {
     return response;
 }
 
+// ===== RANGE DATA PARSING =====
+
 bool UWBHelper::parseRangeData(String data, DeviceData devices[], int maxDevices) {
     if (!data.startsWith("AT+RANGE=")) {
         return false;
@@ -108,7 +222,22 @@ bool UWBHelper::parseRangeData(String data, DeviceData devices[], int maxDevices
     if (rangeEnd == -1) return false;
     
     String rangeData = data.substring(rangeStart, rangeEnd);
-    float distance = rangeData.toFloat() / 100.0; // Convert cm to meters
+    
+    // Parse first non-zero distance
+    int commaIdx = 0;
+    float distance = 0.0;
+    for (int i = 0; i < 8; i++) {
+        int nextComma = rangeData.indexOf(',', commaIdx);
+        if (nextComma == -1) nextComma = rangeData.length();
+        
+        float dist = rangeData.substring(commaIdx, nextComma).toFloat();
+        if (dist > 0) {
+            distance = dist / 100.0; // Convert cm to meters
+            break;
+        }
+        commaIdx = nextComma + 1;
+        if (commaIdx >= rangeData.length()) break;
+    }
     
     // Parse RSSI data
     int rssiIndex = data.indexOf("rssi:(");
@@ -137,10 +266,154 @@ bool UWBHelper::parseRangeData(String data, DeviceData devices[], int maxDevices
     return false;
 }
 
-String UWBHelper::getVersion() {
+bool UWBHelper::parseDetailedRangeData(String data, RangeResult* result) {
-    return sendCommand("AT+GETVER?", 2000);
+    if (!data.startsWith("AT+RANGE=")) {
+        return false;
+    }
+    
+    // Parse tid
+    int tidIndex = data.indexOf("tid:");
+    if (tidIndex == -1) return false;
+    int commaPos = data.indexOf(',', tidIndex);
+    if (commaPos == -1) return false;
+    result->tagId = data.substring(tidIndex + 4, commaPos).toInt();
+    
+    // Parse timer (if present)
+    int timerIndex = data.indexOf("timer:");
+    if (timerIndex != -1) {
+        int timerComma = data.indexOf(',', timerIndex);
+        if (timerComma == -1) timerComma = data.indexOf(',', timerIndex);
+        result->timer = data.substring(timerIndex + 6, timerComma).toInt();
+    }
+    
+    // Parse timerSys (if present)
+    int timerSysIndex = data.indexOf("timerSys:");
+    if (timerSysIndex != -1) {
+        int timerSysComma = data.indexOf(',', timerSysIndex);
+        if (timerSysComma == -1) timerSysComma = data.indexOf(',', timerSysIndex);
+        result->timerSys = data.substring(timerSysIndex + 9, timerSysComma).toInt();
+    }
+    
+    // Parse mask
+    int maskIndex = data.indexOf("mask:");
+    if (maskIndex != -1) {
+        int maskComma = data.indexOf(',', maskIndex);
+        result->mask = data.substring(maskIndex + 5, maskComma).toInt();
+    }
+    
+    // Parse sequence
+    int seqIndex = data.indexOf("seq:");
+    if (seqIndex != -1) {
+        int seqComma = data.indexOf(',', seqIndex);
+        result->sequence = data.substring(seqIndex + 4, seqComma).toInt();
+    }
+    
+    // Parse ranges
+    int rangeIndex = data.indexOf("range:(");
+    if (rangeIndex != -1) {
+        int rangeStart = rangeIndex + 7;
+        int rangeEnd = data.indexOf(')', rangeStart);
+        String rangeData = data.substring(rangeStart, rangeEnd);
+        
+        int startIdx = 0;
+        for (int i = 0; i < 8; i++) {
+            int commaIdx = rangeData.indexOf(',', startIdx);
+            if (commaIdx == -1) commaIdx = rangeData.length();
+            
+            result->ranges[i] = rangeData.substring(startIdx, commaIdx).toFloat() / 100.0;
+            startIdx = commaIdx + 1;
+            if (startIdx >= rangeData.length()) break;
+        }
+    }
+    
+    // Parse RSSI
+    int rssiIndex = data.indexOf("rssi:(");
+    if (rssiIndex != -1) {
+        int rssiStart = rssiIndex + 6;
+        int rssiEnd = data.indexOf(')', rssiStart);
+        String rssiData = data.substring(rssiStart, rssiEnd);
+        
+        int startIdx = 0;
+        for (int i = 0; i < 8; i++) {
+            int commaIdx = rssiData.indexOf(',', startIdx);
+            if (commaIdx == -1) commaIdx = rssiData.length();
+            
+            result->rssi[i] = rssiData.substring(startIdx, commaIdx).toFloat();
+            startIdx = commaIdx + 1;
+            if (startIdx >= rssiData.length()) break;
+        }
+    }
+    
+    // Parse anchor IDs
+    int ancidIndex = data.indexOf("ancid:(");
+    if (ancidIndex != -1) {
+        int ancidStart = ancidIndex + 7;
+        int ancidEnd = data.indexOf(')', ancidStart);
+        String ancidData = data.substring(ancidStart, ancidEnd);
+        
+        int startIdx = 0;
+        for (int i = 0; i < 8; i++) {
+            int commaIdx = ancidData.indexOf(',', startIdx);
+            if (commaIdx == -1) commaIdx = ancidData.length();
+            
+            result->anchorIds[i] = ancidData.substring(startIdx, commaIdx).toInt();
+            startIdx = commaIdx + 1;
+            if (startIdx >= ancidData.length()) break;
+        }
+    }
+    
+    return true;
 }
 
+// ===== ADVANCED FUNCTIONS =====
+
+bool UWBHelper::requestAnchorPosition(int anchorId, AnchorPosition* position) {
+    // Custom command to request anchor position
+    String cmd = "AT+GETPOS=" + String(anchorId);
+    String response = sendCommand(cmd, 2000);
+    
+    if (response.indexOf("POS=") >= 0) {
+        // Parse response format: POS=id,x,y,confidence
+        int idStart = response.indexOf("=") + 1;
+        int comma1 = response.indexOf(",", idStart);
+        int comma2 = response.indexOf(",", comma1 + 1);
+        int comma3 = response.indexOf(",", comma2 + 1);
+        
+        if (comma1 > 0 && comma2 > 0 && comma3 > 0) {
+            position->anchorId = response.substring(idStart, comma1).toInt();
+            position->x = response.substring(comma1 + 1, comma2).toFloat();
+            position->y = response.substring(comma2 + 1, comma3).toFloat();
+            position->confidence = response.substring(comma3 + 1).toFloat();
+            position->valid = true;
+            return true;
+        }
+    }
+    
+    position->valid = false;
+    return false;
+}
+
+bool UWBHelper::calculatePosition(DeviceData devices[], int deviceCount, float* x, float* y) {
+    // Simple trilateration with first 3 active devices
+    int activeCount = 0;
+    DeviceData activeDevices[3];
+    
+    for (int i = 0; i < deviceCount && activeCount < 3; i++) {
+        if (devices[i].active) {
+            activeDevices[activeCount] = devices[i];
+            activeCount++;
+        }
+    }
+    
+    if (activeCount < 3) return false;
+    
+    // For now, assume anchors are at fixed positions (would need anchor position data)
+    // This is a placeholder - real implementation would use actual anchor coordinates
+    return false;
+}
+
+// ===== UTILITY FUNCTIONS =====
+
 bool UWBHelper::isResponseOK(String response) {
     return response.indexOf("OK") >= 0;
 }
@@ -148,12 +421,16 @@ bool UWBHelper::isResponseOK(String response) {
 void UWBHelper::printDiagnostics() {
     Serial.println("=== UWB Diagnostics ===");
     Serial.println("Version: " + getVersion());
-    Serial.println("Config: " + sendCommand("AT+GETCFG?", 2000));
+    Serial.println("Config: " + getConfig());
-    Serial.println("Capacity: " + sendCommand("AT+GETCAP?", 2000));
+    Serial.println("Capacity: " + getCapacity());
-    Serial.println("Power: " + sendCommand("AT+GETPOW?", 2000));
+    Serial.println("Antenna: " + getAntennaDelay());
+    Serial.println("Power: " + getPower());
+    Serial.println("Network: " + getNetwork());
+    Serial.println("Reporting: " + getReporting());
 }
 
-// DistanceFilter Implementation
+// ===== DISTANCE FILTER IMPLEMENTATION =====
+
 DistanceFilter::DistanceFilter() : index(0), filled(false) {
     for (int i = 0; i < FILTER_SIZE; i++) readings[i] = 0;
 }
@@ -195,4 +472,71 @@ void DistanceFilter::reset() {
     index = 0;
     filled = false;
     for (int i = 0; i < FILTER_SIZE; i++) readings[i] = 0;
+}
+
+// ===== POSITION CALCULATOR IMPLEMENTATION =====
+
+bool PositionCalculator::trilaterate(float x1, float y1, float r1,
+                                   float x2, float y2, float r2,
+                                   float x3, float y3, float r3,
+                                   float* x, float* y) {
+    // Trilateration algorithm
+    float A = 2 * (x2 - x1);
+    float B = 2 * (y2 - y1);
+    float C = pow(r1, 2) - pow(r2, 2) - pow(x1, 2) + pow(x2, 2) - pow(y1, 2) + pow(y2, 2);
+    float D = 2 * (x3 - x2);
+    float E = 2 * (y3 - y2);
+    float F = pow(r2, 2) - pow(r3, 2) - pow(x2, 2) + pow(x3, 2) - pow(y2, 2) + pow(y3, 2);
+    
+    float denominator = A * E - B * D;
+    if (abs(denominator) < 0.001) return false; // Points are collinear
+    
+    *x = (C * E - F * B) / denominator;
+    *y = (A * F - D * C) / denominator;
+    
+    return true;
+}
+
+bool PositionCalculator::multilaterate(AnchorPosition anchors[], float distances[], int count, float* x, float* y) {
+    if (count < 3) return false;
+    
+    // Use least squares method for more than 3 anchors
+    if (count == 3) {
+        return trilaterate(anchors[0].x, anchors[0].y, distances[0],
+                          anchors[1].x, anchors[1].y, distances[1],
+                          anchors[2].x, anchors[2].y, distances[2],
+                          x, y);
+    }
+    
+    // For more than 3 anchors, use weighted least squares (simplified version)
+    float sumX = 0, sumY = 0, sumW = 0;
+    
+    for (int i = 0; i < count - 1; i++) {
+        for (int j = i + 1; j < count; j++) {
+            for (int k = j + 1; k < count; k++) {
+                float tempX, tempY;
+                if (trilaterate(anchors[i].x, anchors[i].y, distances[i],
+                               anchors[j].x, anchors[j].y, distances[j],
+                               anchors[k].x, anchors[k].y, distances[k],
+                               &tempX, &tempY)) {
+                    float weight = anchors[i].confidence * anchors[j].confidence * anchors[k].confidence;
+                    sumX += tempX * weight;
+                    sumY += tempY * weight;
+                    sumW += weight;
+                }
+            }
+        }
+    }
+    
+    if (sumW > 0) {
+        *x = sumX / sumW;
+        *y = sumY / sumW;
+        return true;
+    }
+    
+    return false;
+}
+
+float PositionCalculator::calculateDistance(float x1, float y1, float x2, float y2) {
+    return sqrt(pow(x2 - x1, 2) + pow(y2 - y1, 2));
 }

lib/UWBHelper/UWBHelper.h

@@ -12,6 +12,25 @@ struct DeviceData {
     bool active;
 };
 
+struct AnchorPosition {
+    int anchorId;
+    float x;
+    float y;
+    float confidence;
+    bool valid;
+};
+
+struct RangeResult {
+    int tagId;
+    int mask;
+    int sequence;
+    float ranges[8];
+    float rssi[8];
+    int anchorIds[8];
+    unsigned long timer;
+    unsigned long timerSys;
+};
+
 class UWBHelper {
 private:
     HardwareSerial* uwbSerial;
@@ -20,26 +39,68 @@ private:
 public:
     UWBHelper(HardwareSerial* serial, int reset);
     
-    // Initialization
+    // Basic Commands (3.2-3.6)
+    bool testConnection();                              // AT?
+    String getVersion();                                // AT+GETVER?
+    bool restart();                                     // AT+RESTART
+    bool restore();                                     // AT+RESTORE
+    bool save();                                        // AT+SAVE
+    
+    // Configuration Commands (3.7-3.8)
+    bool setConfig(int deviceId, int role, int dataRate = 1, int rangeFilter = 1);  // AT+SETCFG
+    String getConfig();                                 // AT+GETCFG?
+    
+    // Antenna Commands (3.9-3.10)
+    bool setAntennaDelay(int delay);                    // AT+SETANT
+    String getAntennaDelay();                           // AT+GETANT?
+    
+    // Capacity Commands (3.11-3.12)
+    bool setCapacity(int tagCount, int timeSlot = 10, int extMode = 0);  // AT+SETCAP
+    String getCapacity();                               // AT+GETCAP?
+    
+    // Reporting Commands (3.13-3.14)
+    bool setReporting(bool enable);                     // AT+SETRPT
+    String getReporting();                              // AT+GETRPT?
+    
+    // Range Command (3.15)
+    bool parseRangeData(String data, DeviceData devices[], int maxDevices);
+    bool parseDetailedRangeData(String data, RangeResult* result);
+    
+    // Sleep Command (3.16)
+    bool setSleep(int sleepTime);                       // AT+SLEEP
+    
+    // Power Commands (3.17-3.18)
+    bool setPower(String powerValue = "FD");            // AT+SETPOW
+    String getPower();                                  // AT+GETPOW?
+    
+    // Data Commands (3.19-3.20)
+    bool sendData(int length, String data);             // AT+DATA
+    String receiveData();                               // AT+RDATA
+    
+    // Network Commands (3.21-3.22)
+    bool setNetwork(int networkId);                     // AT+SETPAN
+    String getNetwork();                                // AT+GETPAN?
+    
+    // Legacy wrapper functions for backward compatibility
     bool begin();
     void reset();
-    
-    // Configuration
     bool configureDevice(int deviceId, bool isAnchor, int dataRate = 1, int rangeFilter = 1);
-    bool setCapacity(int tagCount, int timeSlot = 10, int extMode = 1);
-    bool setNetwork(int networkId);
     bool enableReporting(bool enable);
     bool saveConfiguration();
     bool restartDevice();
     
     // Communication
     String sendCommand(String command, int timeout = 2000);
-    bool parseRangeData(String data, DeviceData devices[], int maxDevices);
+    
+    // Advanced parsing
+    bool requestAnchorPosition(int anchorId, AnchorPosition* position);
     
     // Utility
-    String getVersion();
     bool isResponseOK(String response);
     void printDiagnostics();
+    
+    // Position calculation helpers
+    bool calculatePosition(DeviceData devices[], int deviceCount, float* x, float* y);
 };
 
 // Data filtering class
@@ -57,4 +118,16 @@ public:
     void reset();
 };
 
+// Position calculation class
+class PositionCalculator {
+public:
+    static bool trilaterate(float x1, float y1, float r1,
+                           float x2, float y2, float r2,
+                           float x3, float y3, float r3,
+                           float* x, float* y);
+    
+    static bool multilaterate(AnchorPosition anchors[], float distances[], int count, float* x, float* y);
+    static float calculateDistance(float x1, float y1, float x2, float y2);
+};
+
 #endif // UWBHELPER_H