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

#include <armor_finder/armor_finder.h>
#include <additions.h>
#include <log.h>
#include <show_images/ballistic_predicition.h>
#include <config/setconfig.h>
#include <opencv2/core.hpp>

static bool fitCircle(const std::vector<ArmorFinder::HistoryItem>& history, cv::Point3f& center, double& radius) {
    int n = history.size();
    if (n < 15) return false;
    cv::Mat A(n, 3, CV_64F);
    cv::Mat B(n, 1, CV_64F);
    double sum_y = 0;
    for (int i = 0; i < n; i++) {
        double x = history[i].pos.x;
        double z = history[i].pos.z;
        sum_y += history[i].pos.y;
        A.at<double>(i, 0) = -2.0 * x;
        A.at<double>(i, 1) = -2.0 * z;
        A.at<double>(i, 2) = 1.0;
        B.at<double>(i, 0) = -(x * x + z * z);
    }
    cv::Mat X;
    if (!cv::solve(A, B, X, cv::DECOMP_SVD)) return false;
    
    center.x = X.at<double>(0, 0);
    center.z = X.at<double>(1, 0);
    center.y = sum_y / n; // Average height
    double c = X.at<double>(2, 0);
    radius = sqrt(center.x * center.x + center.z * center.z - c);
    return true;
}

void ArmorFinder::antiTop(double dist_m, double pitch_imu_deg) {
    if (target_box.rect == cv::Rect2d() || history.empty()) return;

    // 1. Fit Circle to find center
    cv::Point3f center;
    double radius;
    bool has_center = fitCircle(history, center, radius);

    double curr_time = frame_time / 1000.0; // time in seconds

    if (!has_center) {
        // Not enough data to fit circle. Aim at current target.
        double yaw = atan2(target_xyz.x, target_xyz.z) * 180 / PI;
        sendAntiTopTarget(yaw, 0, false);
        return;
    }

    // 2. Lock Gimbal Yaw to the Center
    double center_yaw = atan2(center.x, center.z) * 180 / PI;

    // 3. Phase Analysis and Omega Calculation
    double dx = target_xyz.x - center.x;
    double dz = target_xyz.z - center.z;
    double current_phase = atan2(dz, dx); // radians, [-pi, pi]

    double dt = curr_time - (last_phase_time == 0 ? curr_time : last_phase_time);
    if (dt > 0.005 && dt < 0.1) {
        double d_phase = current_phase - last_phase;
        while (d_phase > PI) d_phase -= 2*PI;
        while (d_phase < -PI) d_phase += 2*PI;
        
        double current_omega = d_phase / dt;
        if (auto_omega == 0) {
            auto_omega = current_omega;
        } else {
            auto_omega = 0.8 * auto_omega + 0.2 * current_omega; // Low pass filter
        }
    }
    last_phase = current_phase;
    last_phase_time = curr_time;

    // 4. Calculate prediction
    if (abs(auto_omega) < 1.0) { // Top is not spinning fast enough or noise
        sendAntiTopTarget(center_yaw, 0, false);
        return;
    }

    // The vector from center to camera is (-center.x, -center.z).
    // The plate points at the camera when it's exactly between center and camera.
    double camera_phase = atan2(-center.z, -center.x); 
    
    // Find min time to hit ANY of the 4 plates
    double min_t_hit = 1e9;
    for (int k = 0; k < 4; k++) {
        double p = current_phase + k * PI / 2.0;
        double diff = camera_phase - p;
        
        while (diff > PI) diff -= 2*PI;
        while (diff < -PI) diff += 2*PI;
        
        if (auto_omega > 0 && diff < 0) diff += 2*PI;
        if (auto_omega < 0 && diff > 0) diff -= 2*PI;
        
        double t_hit = diff / auto_omega; // will be positive
        if (t_hit < min_t_hit && t_hit > 0) {
            min_t_hit = t_hit;
        }
    }

    // 5. Check if it's time to fire
    double fly_time_s = BallisticSolver::get_flight_time(dist_m, pitch_imu_deg, MUZZLE_VELOCITY, BALLISTIC_K);
    double total_delay_s = fly_time_s + SYSTEM_DELAY / 1000.0;

    // 核心优化：O(1) 绝对相位耦合算法
    // 原理：子弹必须在装甲板精确到达 Yaw 中心的那一刻撞击
    double T_face = abs((PI / 2.0) / auto_omega); // 陀螺转过一个面的理论耗时
    
    // 如果我们现在立刻开火，子弹到达的时间点超越了下一块板的到来时间，
    // 意味着我们必然“脱靶”（子弹飞在半空时，最近的板子已经溜过去了）。
    // 在这个差值 time_shortfall 内，我们究竟错过了几块板子？
    double time_shortfall = total_delay_s - min_t_hit; 
    
    double wait_time_s = 0.0;
    if (time_shortfall <= 0) {
        // 第一块板子的时间还没到，完全赶得及！我们只需要原地等差值补齐就行。
        wait_time_s = -time_shortfall; 
    } else {
        // 延迟太高，必定完美错过前面的板子。
        // 通过 ceil（向上取整）推导出“子弹飞到时，我们至少要迎击第 n 块板”：
        int n_plates_missed = ceil(time_shortfall / T_face);
        
        // 我们真实需要等待的时机 = 命中第 n 块板的绝对时间 - 子弹与系统的总延迟
        wait_time_s = (min_t_hit + n_plates_missed * T_face) - total_delay_s;
    }

    bool fire = false;
    // If the time falls within a 20ms prediction window, trigger immediately.
    if (wait_time_s < 0.020 && wait_time_s >= -0.010) {
        fire = true;
    }

    uint16_t shoot_delay = static_cast<uint16_t>(wait_time_s * 1000.0);
    sendAntiTopTarget(center_yaw, shoot_delay, fire);
}

