#include "i2c_communication.h"
#include <TinyGPS++.h>
#include "main.h"

extern uint16_t receive_cnt;

extern TinyGPSPlus gps;
extern uint16_t distance;
// 缓冲区用于发送结构体数据
uint8_t i2c_tx_buffer[sizeof(hh3_slave_data)];

// 初始化 I2C 从机
void init_i2c_slave() {
    Wire.setSDA(I2C_SDA_PIN);
    Wire.setSCL(I2C_SCL_PIN);
    //Wire.Pins(16,17);
    Wire.begin(SLAVE_ADDRESS);                 // 设置从机地址为 0x55
    Wire.onRequest(onRequestEvent);   // 注册请求回调  ***主机使用命令0x01不行，无应答
    Wire.onReceive(onReceiveEvent);   // 注册接收回调
}

// 当主机请求数据时调用
void onRequestEvent() {
    update_sensor_data(); // 确保数据是最新的
    memcpy(i2c_tx_buffer, &sensor_data, sizeof(sensor_data));
    Wire.write(i2c_tx_buffer, sizeof(sensor_data));
}

void power_init(){
    pinMode(RANGING_POWER_PIN, OUTPUT);
    pinMode(GPS_POWER_PIN, OUTPUT);
}

// 当主机发送命令时调用
void onReceiveEvent(int bytes) {
    while (Wire.available()) {
        uint8_t cmd = Wire.read();
        switch (cmd) {
            case CMD_GPS_POWER_OFF:
                turn_off_gps();
                break;
            case CMD_RANGING_POWER_OFF:
                turn_off_ranging();
                break;
            case CMD_GPS_POWER_ON:
                turn_on_gps();
                break;
            case CMD_RANGING_POWER_ON:
                turn_on_ranging();
                break;
            case CMD_MASTER_GET_SLAVE_DATA: 
                onRequestEvent();  // 主动调用一次数据发送.功能不正常，需使用Wire.onRequest(onRequestEvent);
                break;
            default:
                Serial.printf("Unknown command received: 0x%02X\n", cmd);
                break;
        }
    }
}

// 更新传感器状态和数据
unsigned long last_gps_data_time = 0;
static uint8_t last_receive_cnt = 0;

void update_sensor_data() {
    //gps
    //if (millis() > 5000 && gps.charsProcessed() < 10 && digitalRead(GPS_POWER_PIN) == HIGH) 
    if (gps.charsProcessed() < 10) {
        sensor_data.gps_sta = 0x00; // 无设备
        sensor_data.gps_lat = 0.0f;
        sensor_data.gps_lon = 0.0f;
    } else if (!gps.location.isUpdated()) {
        sensor_data.gps_sta = 0x02; // 未搜到信号
        sensor_data.gps_lat = 0.0f;
        sensor_data.gps_lon = 0.0f;
    } else {
        sensor_data.gps_sta = 0x03; // 正常工作
        sensor_data.gps_lat = gps.location.lat();
        sensor_data.gps_lon = gps.location.lng();
    }

    //测距
    //if (receive_cnt == last_receive_cnt && digitalRead(RANGING_POWER_PIN) == HIGH) 
    if (receive_cnt == last_receive_cnt){
        sensor_data.height_sta = 0x00; // 无设备
        sensor_data.height = 0.0f;
    }else {
        last_receive_cnt = receive_cnt;
        sensor_data.height_sta = 0x03; // 有效高度，正常工作
        sensor_data.height = (float)distance / 1000.0f; // mm -> m
    }
}

// 电源控制
void turn_off_gps() {
    digitalWrite(GPS_POWER_PIN, LOW);
    //Serial.println("Turning OFF GPS Power");
}

void turn_on_gps() {
    digitalWrite(GPS_POWER_PIN, HIGH);
    //Serial.println("Turning ON GPS Power");
}

void turn_off_ranging() {
    digitalWrite(RANGING_POWER_PIN, LOW);
    //Serial.println("Turning OFF Ranging Module");
}

void turn_on_ranging() {
    digitalWrite(RANGING_POWER_PIN, HIGH);
    //Serial.println("Turning ON Ranging Module");
}