#include "navigation.h"
#include <Arduino.h>

// ----------- 内部状态变量 -----------
static NavigationState current_nav_state = NAV_IDLE;
static int detected_spot_count = 0;
static bool side_spot_debounce = false; // 用于侧向超声波检测去抖
static unsigned long last_side_spot_detect_time = 0;
const unsigned long SPOT_DEBOUNCE_TIME_MS = 2000; // 2秒去抖时间，避免重复计数同一个桩

// 循迹PID相关 (如果使用)
static float pid_error = 0;
static float pid_last_error = 0;
static float pid_integral = 0;
static float pid_derivative = 0;

// 目标车位ID的映射 (从config.h中获取)
const char* SPOT_IDS[NUM_CHARGING_SPOTS] = {SPOT_ID_1, SPOT_ID_2, SPOT_ID_3};

// 将字符串的 spot_id (如 "001") 转换为索引 (0, 1, 2)
int get_spot_index(const char* target_spot_uid) {
    if (!target_spot_uid) return -1;
    for (int i = 0; i < NUM_CHARGING_SPOTS; ++i) {
        if (strcmp(target_spot_uid, SPOT_IDS[i]) == 0) {
            return i;
        }
    }
    return -1; // 未找到
}

void navigation_setup() {
    current_nav_state = NAV_IDLE;
    detected_spot_count = 0;
    side_spot_debounce = false;
    // 初始化PID变量 (如果使用)
    pid_error = 0; pid_last_error = 0; pid_integral = 0; pid_derivative = 0;
    Serial.println("导航系统已初始化。");
}

void navigation_reset_spot_counter() {
    detected_spot_count = 0;
    side_spot_debounce = false;
    Serial.println("车位计数器已重置。");
}

int navigation_get_current_spot_count() {
    return detected_spot_count;
}

NavigationState navigation_get_state() {
    return current_nav_state;
}

void navigation_follow_line_once() {
    LineFollowValues line_values = get_line_follow_values();
    int base_speed = 150; // 基础循迹速度，需要调整
    int turn_speed_diff = 80; // 转向时的速度差或舵机调整量，需要调整
    int servo_turn_angle_small = 15; // 小幅调整舵机角度 (例如10-20度)
    int servo_turn_angle_large = 30; // 大幅调整舵机角度

    // 这是一个非常基础的循迹逻辑，需要根据实际传感器和机器人进行大幅调整
    // 您可能需要更复杂的PID控制或其他算法

    if (line_values.left_on_line && line_values.right_on_line) {
        // 1. 都在线上 (或者在两条线的中间)，直行
        servo_center();
        motor_move_forward(base_speed);
        // Serial.println("Nav: Straight");
    } else if (line_values.left_on_line && !line_values.right_on_line) {
        // 2. 左边在线，右边不在：车体偏右，需要向左调整
        // motor_set_raw_speeds(base_speed - turn_speed_diff, base_speed + turn_speed_diff); // 左轮慢，右轮快
        servo_set_angle(90 - servo_turn_angle_small); // 舵机向左
        motor_move_forward(base_speed * 0.8); // 调整时可以稍微减速
        // Serial.println("Nav: Turn Left (correcting rightward drift)");
    } else if (!line_values.left_on_line && line_values.right_on_line) {
        // 3. 右边在线，左边不在：车体偏左，需要向右调整
        // motor_set_raw_speeds(base_speed + turn_speed_diff, base_speed - turn_speed_diff); // 左轮快，右轮慢
        servo_set_angle(90 + servo_turn_angle_small); // 舵机向右
        motor_move_forward(base_speed * 0.8);
        // Serial.println("Nav: Turn Right (correcting leftward drift)");
    } else {
        // 4. 两边都不在线：可能偏离太远，或者到达线路末端/交叉口
        // 简单处理：停止或尝试最后的方向转动
        motor_stop(); // 或者执行更复杂的搜索线路逻辑
        Serial.println("Nav: Both sensors off line! Stopping.");
        // current_nav_state = NAV_ERROR; // 可以设置错误状态
    }
    // PID循迹逻辑 (示例，未完全集成)
    /*
    if (line_values.left_on_line && !line_values.right_on_line) pid_error = -1; // 偏右
    else if (!line_values.left_on_line && line_values.right_on_line) pid_error = 1;  // 偏左
    else if (line_values.left_on_line && line_values.right_on_line) pid_error = 0; // 居中
    else { // 都脱离，根据上次误差方向大幅调整或停车
        pid_error = pid_last_error * 2; // 放大上次误差
    }
    pid_integral += pid_error;
    pid_derivative = pid_error - pid_last_error;
    float motor_adjustment = KP * pid_error + KI * pid_integral + KD * pid_derivative;
    pid_last_error = pid_error;
    int left_motor_speed = base_speed - motor_adjustment;
    int right_motor_speed = base_speed + motor_adjustment;
    servo_set_angle(90 - (int)(motor_adjustment * 10)); // 假设舵机调整与motor_adjustment相关
    motor_set_raw_speeds(left_motor_speed, right_motor_speed);
    */
}

bool navigation_check_obstacle() {
    long dist = get_front_obstacle_distance();
    if (dist > 0 && dist < 20) { // 假设前方小于20cm为障碍物
        Serial.print("Nav: Obstacle detected at "); Serial.print(dist); Serial.println(" cm");
        return true;
    }
    return false;
}

void navigation_handle_obstacle() {
    Serial.println("Nav: Handling obstacle...");
    motor_stop();
    // 简单处理：可以尝试后退一点，然后再次尝试，或者等待一段时间
    // 更复杂的避障逻辑（如转向绕过）需要更多传感器和算法
    // For now, just stop and set state to allow intervention or timeout
    current_nav_state = NAV_AVOIDING_OBSTACLE; // 更新导航内部状态
    // 在实际应用中，这里可能需要通知主循环机器人状态为FAULTED或需要帮助
}

// 返回true表示到达目标，false表示未到达或已过目标
bool navigation_check_spot_arrival(const char* target_spot_uid) {
    long side_dist = get_side_spot_distance();
    int target_spot_index = get_spot_index(target_spot_uid);

    if (target_spot_index < 0) {
        Serial.print("Nav Error: Invalid target_spot_uid for arrival check: "); Serial.println(target_spot_uid);
        return false; // 无效目标
    }

    if (side_dist > 0 && side_dist < SIDE_ULTRASONIC_SPOT_THRESHOLD_CM) {
        if (!side_spot_debounce) {
            detected_spot_count++;
            side_spot_debounce = true; // 进入去抖状态
            last_side_spot_detect_time = millis();
            Serial.print("Nav: Detected spot candidate. Count: "); Serial.println(detected_spot_count);
            
            // 检查是否到达了目标充电桩 (目标是第 target_spot_index + 1 个桩)
            if (detected_spot_count == (target_spot_index + 1)) {
                Serial.print("Nav: TARGET SPOT [" 
                Serial.println("Nav: TARGET SPOT [" + String(target_spot_uid) + "] REACHED (Count matched)!");
                return true; // 到达目标
            }
        }
    } else {
        // 如果距离大于阈值，且距离上次检测到满足条件已经超过去抖时间，则解除去抖
        if (side_spot_debounce && (millis() - last_side_spot_detect_time > SPOT_DEBOUNCE_TIME_MS)) {
            side_spot_debounce = false;
            Serial.println("Nav: Side spot debounce period ended.");
        }
    }
    
    // 如果已经计数的桩超过了目标桩的索引+1，说明可能错过了，或者目标逻辑有问题
    if (detected_spot_count > (target_spot_index + 1)) {
        Serial.println("Nav Warning: Detected spot count exceeds target. Possible overshoot or error.");
        // current_nav_state = NAV_ERROR; // 可以设置为错误状态
        // return false; // 或者根据业务逻辑，这里也视为一种失败
    }

    return false; // 未到达目标
}


bool navigation_move_to_spot(const char* target_spot_uid, RobotStatus* current_robot_status_ptr, String* current_location_ptr) {
    if (!target_spot_uid || !current_robot_status_ptr || !current_location_ptr) {
        Serial.println("Nav Error: Invalid arguments to navigation_move_to_spot.");
        return false;
    }

    Serial.print("Nav: Starting move_to_spot for target: "); Serial.println(target_spot_uid);
    *current_robot_status_ptr = MOVING;
    *current_location_ptr = "EN_ROUTE_TO_" + String(target_spot_uid);
    current_nav_state = NAV_FOLLOWING_LINE;
    // navigation_reset_spot_counter(); // 在开始新的导航任务时重置计数器
                                       // 注意: 重置计数器的时机很重要。如果机器人从任意位置启动，则应重置。
                                       // 如果是连续任务的一部分，则可能不需要。
                                       // 假设从基站出发或新任务开始，这里重置。
                                       // 但如果是在多个桩之间移动，这个逻辑需要调整。
                                       // 为了简单，我们假设每次MOVE指令都是一个新的完整寻路。
    if (detected_spot_count != 0) { // 如果不是从0开始，说明上次任务可能未清理或逻辑错误
        Serial.println("Nav Warning: Spot counter not zero at start of new navigation. Resetting.");
        navigation_reset_spot_counter();
    }


    unsigned long nav_start_time = millis();
    unsigned long MAX_NAV_TIME_MS = 120000; // 例如，最大导航时间2分钟，防止卡死

    while (current_nav_state != NAV_TARGET_REACHED && current_nav_state != NAV_ERROR) {
        if (millis() - nav_start_time > MAX_NAV_TIME_MS) {
            Serial.println("Nav Error: Navigation timeout!");
            motor_stop();
            current_nav_state = NAV_ERROR;
            *current_robot_status_ptr = FAULTED; // 更新主状态
            *current_location_ptr = "LOST_OR_TIMEOUT";
            break;
        }

        if (navigation_check_obstacle()) {
            navigation_handle_obstacle();
            *current_robot_status_ptr = MOVING; // 仍然是移动中，但可能需要人工介入或等待
            *current_location_ptr = "OBSTACLE_AVOIDANCE";
            // 在简单避障后，可能需要重新进入 NAV_FOLLOWING_LINE 状态
            // motor_stop(); // 确保在避障时机器人停止，或执行避障动作
            // delay(1000); // 等待
            // current_nav_state = NAV_FOLLOWING_LINE; // 尝试继续 (这部分逻辑需要完善)
            // 目前简单处理是停止，并依赖外部超时或指令取消
            Serial.println("Nav: Obstacle detected, stopping and awaiting further logic/timeout.");
            motor_stop();
            // break; // 或者跳出循环，让外部处理
            // 暂不break，允许其他状态检查或MQTT消息进来中断
        }
        
        if (current_nav_state == NAV_FOLLOWING_LINE || current_nav_state == NAV_AT_SPOT_CANDIDATE) {
            navigation_follow_line_once();
            if (navigation_check_spot_arrival(target_spot_uid)) {
                current_nav_state = NAV_TARGET_REACHED;
                *current_location_ptr = String(target_spot_uid); // 更新位置为目标车位
            }
        }

        // (这里可以加入MQTT消息的检查，如果收到STOP指令，可以中断导航)
        // mqtt_loop(); // 不建议在阻塞函数中频繁调用mqtt_loop，应由主循环处理

        delay(50); // 短暂延时，控制循环频率
    }

    motor_stop(); // 确保在函数结束时电机停止

    if (current_nav_state == NAV_TARGET_REACHED) {
        Serial.print("Nav: Successfully reached target "); Serial.println(target_spot_uid);
        return true;
    } else {
        Serial.print("Nav: Failed to reach target "); Serial.println(target_spot_uid);
        return false;
    }
}