#ifndef UWBHELPER_H
#define UWBHELPER_H

#include <Arduino.h>
#include <HardwareSerial.h>

struct DeviceData {
    int deviceId;
    float distance;
    float rssi;
    unsigned long lastUpdate;
    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;
    int resetPin;
    
public:
    UWBHelper(HardwareSerial* serial, int reset);
    
    // 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 isAnchor = true);
    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();
    bool configureDevice(int deviceId, bool isAnchor, int dataRate = 1, int rangeFilter = 1);
    bool enableReporting(bool enable);
    bool saveConfiguration();
    bool restartDevice();
    
    // Communication
    String sendCommand(String command, int timeout = 2000);
    
    // Advanced parsing
    bool requestAnchorPosition(int anchorId, AnchorPosition* position);
    
    // Utility
    bool isResponseOK(String response);
    void printDiagnostics();
    
    // Position calculation helpers
    bool calculatePosition(DeviceData devices[], int deviceCount, float* x, float* y);
};

// Data filtering class
class DistanceFilter {
private:
    static const int FILTER_SIZE = 5;
    float readings[FILTER_SIZE];
    int index;
    bool filled;
    
public:
    DistanceFilter();
    float addReading(float distance);
    float getFilteredValue();
    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