#include <Wire.h>
#include <Arduino.h>

// 从机地址（与从机中 Wire.begin(0x55) 一致）
#define SLAVE_ADDRESS 0x55

// 命令定义（与从机一致）
#define CMD_MASTER_GET_SLAVE_DATA  0x01
#define CMD_GPS_POWER_OFF     0x02
#define CMD_RANGING_POWER_OFF 0x03
#define CMD_GPS_POWER_ON      0x04
#define CMD_RANGING_POWER_ON  0x05


// 数据结构定义（必须与从机一致）
typedef struct {
    uint8_t gps_sta;       // GPS 状态
    uint8_t height_sta;    // 测距状态
    float gps_lat;
    float gps_lon;
    float height;
} hh3_slave_data;

hh3_slave_data sensor_data;

// 发送命令给从机
void sendCommand(uint8_t cmd) {
    Wire.beginTransmission(SLAVE_ADDRESS);
    Wire.write(cmd);
    Wire.endTransmission();
    Serial.print("Sent command: 0x");
    Serial.println(cmd, HEX);
}


//请求并读取传感器数据
bool requestData() {
    // 可选：先发送请求命令（如果在从机中使用了 CMD_）
    //sendCommand(CMD_MASTER_GET_SLAVE_DATA);

    // 直接请求结构体大小的数据
    Wire.requestFrom(SLAVE_ADDRESS, sizeof(hh3_slave_data));

    if (Wire.available() == sizeof(hh3_slave_data)) {
        Wire.readBytes((uint8_t*)&sensor_data, sizeof(sensor_data));
        return true;
    } else {
        Serial.println("Failed to receive data from slave.");
        return false;
    }
}

// // 请求并读取传感器数据
// #define size sizeof(hh3_slave_data)
// uint8_t data[sizeof(hh3_slave_data)];
// bool requestData() {
//   Wire.beginTransmission(SLAVE_ADDRESS);
//   Wire.write(0x01);
//   Wire.endTransmission();
//   Wire.requestFrom(SLAVE_ADDRESS, size);
//   for (uint8_t i=0; i<size; i++) {
//   data[i] = Wire.read();
//   if( data[i] == -1) return false;
//   return true;
//   }
// }


// 打印传感器数据
void printSensorData() {

    // 打印原始 hex 数据
    uint8_t* pData = (uint8_t*)&sensor_data;
    Serial.print("Received Hex Data: ");
    for (int i = 0; i < sizeof(hh3_slave_data); i++) {
        Serial.printf("%02X ", pData[i]);
    }
    Serial.println();


    Serial.println("=== Sensor Data ===");

    Serial.print("GPS Status: ");
    switch (sensor_data.gps_sta) {
        case 0x00: Serial.println("No Device"); break;
        case 0x01: Serial.println("Power Off"); break;
        case 0x02: Serial.println("No Signal"); break;
        case 0x03: Serial.println("Valid Signal"); break;
        default:   Serial.println("Unknown"); break;
    }

    Serial.print("Height Status: ");
    switch (sensor_data.height_sta) {
        case 0x00: Serial.println("No Device"); break;
        case 0x01: Serial.println("Power Off"); break;
        case 0x02: Serial.println("Valid"); break;
        default:   Serial.println("Unknown"); break;
    }

    Serial.print("Latitude: ");  Serial.println(sensor_data.gps_lat, 6);
    Serial.print("Longitude: "); Serial.println(sensor_data.gps_lon, 6);
    Serial.print("Height (m): "); Serial.println(sensor_data.height, 2);

    Serial.println("===================");
}


void setup() {
    Serial.begin(115200);
    //Wire.setPins(1,2);
    Wire.begin(1,2);

    Serial.println("I2C Master Started...");
    delay(1000);
}

// void loop() {

//     // 请求传感器数据
//     if (requestData()) {
//         printSensorData();
//     }

//     delay(2000);

//     // sendCommand(0x02);
//     // delay(2000);
//     // sendCommand(0x04);
// }

#define INTERVAL 5000

unsigned long previousMillis = 0; // 上次执行的时间点
bool isGpsOn = true;              // GPS 状态标志

void loop() {
    unsigned long currentMillis = millis();

    // 检查是否达到时间间隔
    if (currentMillis - previousMillis >= INTERVAL) {
        previousMillis = currentMillis;

        // 切换状态
        if (isGpsOn) {
            sendCommand(CMD_GPS_POWER_OFF);      // 关闭 GPS
            sendCommand(CMD_RANGING_POWER_OFF);  // 关闭测距
            Serial.println("CLOSE...");
        } else {
            sendCommand(CMD_GPS_POWER_ON);       // 开启 GPS
            sendCommand(CMD_RANGING_POWER_ON);   // 开启测距
             Serial.println("OPEN...");
        }

        isGpsOn = !isGpsOn; // 翻转状态
    }

    // 请求传感器数据并打印
    if (requestData()) {
        printSensorData();
    }
    delay(1500);
}

