//
// Created by xinyang on 19-7-11.
//

#include <armor_finder/armor_finder.h>
#include <config/setconfig.h>
#include <log.h>
#include <chrono>
#include <thread>
#include <mutex>
#include <atomic>

// amadeus_26 协议: 15字节帧
// | 0xBB | 0x77 | x_move(2B) | y_move(2B) | yaw(2B) | pitch(2B) | feed(2B) | switch(1B) | CRC8 | 0xEE |
constexpr uint8_t FRAME_HEADER_1 = 0xBB;
constexpr uint8_t FRAME_HEADER_2 = 0x77;
constexpr uint8_t FRAME_TAIL = 0xEE;
constexpr int FRAME_LENGTH = 15;
constexpr int SEND_INTERVAL_MS = 20;  // 50Hz

// 状态定义
enum TargetState {
    HAVE_TARGET,           // 有目标
    LOST_TARGET_WAIT,      // 丢失目标，等待1秒
    SEARCHING              // 索敌状态（持续旋转直到找到目标）
};

// 共享控制数据
struct ControlData {
    int16_t yaw = 0;
    int16_t pitch = 0;
    int16_t feed = 0;
    uint8_t left_switch = 3;
    uint8_t right_switch = 3;
};

static std::mutex control_mutex;
static ControlData shared_control;
static std::atomic<bool> sender_running{false};
static std::thread sender_thread;

// 更新控制数据
void updateControl(int16_t yaw, int16_t pitch, int16_t feed, 
                   uint8_t left_sw = 3, uint8_t right_sw = 3) {
    std::lock_guard<std::mutex> lock(control_mutex);
    shared_control.yaw = yaw;
    shared_control.pitch = pitch;
    shared_control.feed = feed;
    shared_control.left_switch = left_sw;
    shared_control.right_switch = right_sw;
}

// 发送线程函数
void senderLoop(Serial &serial) {
    uint8_t tx_frame[FRAME_LENGTH];
    
    while (sender_running) {
        auto start = std::chrono::steady_clock::now();
        
        // 获取控制数据（加锁）
        ControlData local_control;
        {
            std::lock_guard<std::mutex> lock(control_mutex);
            local_control = shared_control;
        }
        
        // 构建发送数据帧
        int idx = 0;
        tx_frame[idx++] = FRAME_HEADER_1;
        tx_frame[idx++] = FRAME_HEADER_2;
        
        // x_move, y_move (固定为0)
        tx_frame[idx++] = 0;
        tx_frame[idx++] = 0;
        tx_frame[idx++] = 0;
        tx_frame[idx++] = 0;
        
        // yaw (小端序)
        tx_frame[idx++] = local_control.yaw & 0xFF;
        tx_frame[idx++] = (local_control.yaw >> 8) & 0xFF;
        
        // pitch (小端序)
        tx_frame[idx++] = local_control.pitch & 0xFF;
        tx_frame[idx++] = (local_control.pitch >> 8) & 0xFF;
        
        // feed (小端序)
        tx_frame[idx++] = local_control.feed & 0xFF;
        tx_frame[idx++] = (local_control.feed >> 8) & 0xFF;
        
        // switch
        tx_frame[idx++] = (local_control.left_switch << 4) | local_control.right_switch;
        
        // CRC8
        tx_frame[idx++] = 0xCC;
        
        // 帧尾
        tx_frame[idx++] = FRAME_TAIL;
        
        // 发送数据
        serial.WriteData(tx_frame, FRAME_LENGTH);
        
        // 精确控制发送频率
        auto elapsed = std::chrono::duration_cast<std::chrono::milliseconds>(
            std::chrono::steady_clock::now() - start).count();
        int sleep_ms = SEND_INTERVAL_MS - elapsed;
        if (sleep_ms > 0) {
            std::this_thread::sleep_for(std::chrono::milliseconds(sleep_ms));
        }
    }
}

// 启动发送线程
void startSender(Serial &serial) {
    if (!sender_running) {
        sender_running = true;
        sender_thread = std::thread(senderLoop, std::ref(serial));
        LOGM(STR_CTR(WORD_GREEN, "Sender thread started (50Hz)"));
    }
}

bool ArmorFinder::sendBoxPosition(uint16_t shoot_delay) {
    // 确保发送线程已启动
    static bool sender_started = false;
    if (!sender_started) {
        startSender(serial);
        sender_started = true;
    }
    
    // 状态管理
    static TargetState state = HAVE_TARGET;
    static auto state_start_time = std::chrono::steady_clock::now();
    static int16_t pitch_scan = -100;
    static int pitch_direction = 1;
    
    // 检查当前是否有目标
    bool has_target = (target_box.rect != cv::Rect2d());
    auto now = std::chrono::steady_clock::now();
    
    // 状态机处理
    switch (state) {
        case HAVE_TARGET:
            if (!has_target) {
                // 目标丢失，进入等待状态
                state = LOST_TARGET_WAIT;
                state_start_time = now;
                LOGM(STR_CTR(WORD_YELLOW, "Target lost, waiting 1s before searching..."));
            }
            break;
            
        case LOST_TARGET_WAIT:
            if (has_target) {
                // 目标恢复
                state = HAVE_TARGET;
                LOGM(STR_CTR(WORD_GREEN, "Target reacquired"));
            } else {
                // 检查是否等待满1秒
                auto elapsed = std::chrono::duration_cast<std::chrono::milliseconds>(now - state_start_time).count();
                if (elapsed >= 1000) {  // 1秒到，进入索敌状态
                    state = SEARCHING;
                    LOGM(STR_CTR(WORD_LIGHT_CYAN, "Entering search mode (continuous)"));
                }
            }
            break;
            
        case SEARCHING:
            if (has_target) {
                // 目标恢复，退出索敌
                state = HAVE_TARGET;
                LOGM(STR_CTR(WORD_GREEN, "Target found, exit search mode"));
            }
            // 否则持续索敌，不退出
            break;
    }
    
    // 根据状态更新控制值
    if (state == HAVE_TARGET) {
        // 有目标时的正常处理
        if (shoot_delay) {
            LOGM(STR_CTR(WORD_BLUE, "next box %dms"), shoot_delay);
        }
        auto rect = target_box.rect;
        double dx = rect.x + rect.width / 2 - IMAGE_CENTER_X;
        double dy = rect.y + rect.height / 2 - IMAGE_CENTER_Y;

        // PID
        sum_yaw += dx;
        sum_pitch += dy;
        double tmp_yaw = dx;
        double tmp_pitch = dy;
        dx = YAW_AIM_KP * dx + YAW_AIM_KI * sum_yaw +
                       YAW_AIM_KD * (dx - last_yaw);
        dy = PITCH_AIM_KP * dy + PITCH_AIM_KI * sum_pitch +
                         PITCH_AIM_KD * (dy - last_pitch);

        last_yaw = tmp_yaw;
        last_pitch = tmp_pitch;

        double yaw = atan(dx / FOCUS_PIXAL) * 180 / PI;
        double pitch = atan(dy / FOCUS_PIXAL) * 180 / PI;
        
        // 转换为杆量值（注意：pitch 方向修正）
        int16_t yaw_val = static_cast<int16_t>(yaw * 100);
        int16_t pitch_val = static_cast<int16_t>(-pitch * 100);  // 取反修正方向
        int16_t feed_val = shoot_delay > 0 ? 660 : 0;
        
        // 更新控制数据
        updateControl(yaw_val, pitch_val, feed_val, 3, 3);
        return true;
        
    } else if (state == LOST_TARGET_WAIT) {
        // 等待1秒期间：保持静止，不旋转
        static int wait_count = 0;
        wait_count++;
        if (wait_count % 50 == 0) {
            auto remaining = 1000 - std::chrono::duration_cast<std::chrono::milliseconds>(now - state_start_time).count();
            LOGM(STR_CTR(WORD_YELLOW, "Waiting... %dms left before search"), remaining > 0 ? remaining : 0);
            wait_count = 0;
        }
        // 等待期间不旋转，发送0值
        updateControl(0, 0, 0, 3, 3);
        return true;
        
    } else {  // SEARCHING
        // 索敌状态：持续旋转直到找到目标，加快pitch扫描速度
        pitch_scan += pitch_direction * 15;  // 从7加快到15
        if (pitch_scan >= 200) pitch_direction = -1;
        if (pitch_scan <= -100) pitch_direction = 1;
        
        static int search_count = 0;
        search_count++;
        if (search_count % 50 == 0) {
            LOGM(STR_CTR(WORD_LIGHT_CYAN, "Searching: yaw=150, pitch=%d (waiting for target)"), pitch_scan);
            search_count = 0;
        }
        
        // 索敌状态：yaw=150旋转，pitch大范围扫描，小陀螺开启
        updateControl(150, pitch_scan, 0, 3, 2);
        return true;
    }
}