#include "sensor_handler.h"

void sensor_setup() {
    // 超声波传感器引脚设置
    pinMode(ULTRASONIC_FRONT_TRIG_PIN, OUTPUT);
    pinMode(ULTRASONIC_FRONT_ECHO_PIN, INPUT);
    pinMode(ULTRASONIC_SIDE_TRIG_PIN, OUTPUT);
    pinMode(ULTRASONIC_SIDE_ECHO_PIN, INPUT);
    Serial.println("超声波传感器引脚已初始化。");

    // TCRT5000 循迹传感器引脚设置
    // 如果TCRT5000模块的DOUT引脚直接给出高低电平，则设为INPUT
    // 如果是AOUT引脚，需要用analogRead()，引脚模式通常不需要显式设置，但INPUT也可以
    pinMode(TCRT5000_LEFT_PIN, INPUT);
    pinMode(TCRT5000_RIGHT_PIN, INPUT);
    Serial.println("TCRT5000循迹传感器引脚已初始化。");
}

// --- 超声波传感器函数 ---
long get_ultrasonic_distance_cm(int trig_pin, int echo_pin) {
    digitalWrite(trig_pin, LOW);
    delayMicroseconds(2);
    digitalWrite(trig_pin, HIGH);
    delayMicroseconds(10);
    digitalWrite(trig_pin, LOW);
    long duration = pulseIn(echo_pin, HIGH, 30000); // 等待脉冲返回，超时时间30ms (约5米)
    // 声速为 340m/s 或 29.412 µs/cm (1 / (34000 / 1000000 / 2))
    // (duration / 2) * 0.034 = duration * 0.017
    // More precise: duration / 58.8 (for cm)
    if (duration == 0) { // 超时或检测失败
        return -1; // 返回-1表示无效读数或超出范围
    }
    return duration / 58; // Convert duration to cm
}

long get_front_obstacle_distance() {
    return get_ultrasonic_distance_cm(ULTRASONIC_FRONT_TRIG_PIN, ULTRASONIC_FRONT_ECHO_PIN);
}

long get_side_spot_distance() {
    return get_ultrasonic_distance_cm(ULTRASONIC_SIDE_TRIG_PIN, ULTRASONIC_SIDE_ECHO_PIN);
}

// --- TCRT5000 循迹传感器函数 ---

// 辅助函数：根据模拟值判断是否在线上
// 注意: TCRT5000输出特性可能不同，白色反射强->低电压/低模拟值，黑色吸收->高电压/高模拟值
// 或者相反，取决于模块设计。这里的THRESHOLD假设的是：检测到黑线时读数高于阈值。
// 您需要根据实际情况校准和调整逻辑。
bool is_on_line(int sensor_pin, int threshold) {
    int sensor_value = analogRead(sensor_pin);
    // Serial.print("Sensor pin "); Serial.print(sensor_pin); 
    // Serial.print(" value: "); Serial.println(sensor_value);
    return sensor_value > threshold; // 示例：高于阈值是在黑线上
    // 如果您的传感器是检测到黑线时值变小，则应为: return sensor_value < threshold;
}

LineFollowValues get_line_follow_values() {
    LineFollowValues values;
    
    // 如果TCRT5000的DOUT引脚直接连接到数字引脚，则使用digitalRead()
    // values.left_on_line = digitalRead(TCRT5000_LEFT_PIN) == HIGH; // 假设HIGH表示在黑线上
    // values.right_on_line = digitalRead(TCRT5000_RIGHT_PIN) == HIGH;

    // 如果使用AOUT引脚并进行模拟比较：
    values.left_on_line = is_on_line(TCRT5000_LEFT_PIN, TCRT5000_THRESHOLD);
    values.right_on_line = is_on_line(TCRT5000_RIGHT_PIN, TCRT5000_THRESHOLD);
    
    // 调试输出
    // Serial.print("Line Follow: Left OnLine="); Serial.print(values.left_on_line);
    // Serial.print(", Right OnLine="); Serial.println(values.right_on_line);

    return values;
}