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

#include <armor_finder/armor_finder.h>
#include <config/setconfig.h>
#include <show_images/ballistic_predicition.h>
#include <show_images/auto_trigger.h>
#include <log.h>


static bool sendTarget(Serial &serial, double yaw, uint16_t shoot_delay, bool fire) {
    /*
    uint8_t buff[7];

#ifdef WITH_COUNT_FPS
    static time_t last_time = time(nullptr);
    static int fps;
    time_t t = time(nullptr);
    if (last_time != t) {
        last_time = t;
        cout << "Armor: fps:" << fps << ", yaw: " << yaw << " delay: " << shoot_delay << " fire: " << fire << endl;
        fps = 0;
    }
    fps += 1;
#endif

#define MINMAX(value, min, max) value = ((value) < (min)) ? (min) : ((value) > (max) ? (max) : (value))

    short yaw_tmp = static_cast<short>(yaw * (32768 - 1) / 100);
    
    buff[0] = 's';
    buff[1] = static_cast<char>((yaw_tmp >> 8) & 0xFF);
    buff[2] = static_cast<char>((yaw_tmp >> 0) & 0xFF);
    buff[3] = static_cast<char>((shoot_delay >> 8) & 0xFF);
    buff[4] = static_cast<char>((shoot_delay >> 0) & 0xFF);
    buff[5] = fire ? 1 : 0;    // 1为开火 0为闭嘴
    buff[6] = 'e';
    return serial.WriteData(buff, sizeof(buff));
    */

    // Vofa串口验证 不用可以注释掉
    char buff[128];
    int len = sprintf(buff,"channels: %f, %f, %d\n", yaw, shoot_delay, fire);
    return serial.WriteData((unsigned char *)buff, len);
}

bool ArmorFinder::sendAntiTopTarget(double yaw, uint16_t shoot_delay, bool fire) {
    return sendTarget(serial, yaw, shoot_delay, fire);
}

bool ArmorFinder::sendBoxPosition(uint16_t shoot_delay) {
    if (target_box.rect == cv::Rect2d()) return false;
    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;
    float yaw_I_component = YAW_AIM_KI * sum_yaw;
    float pitch_I_component = PITCH_AIM_KI * sum_pitch;

    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 dist = sqrt(target_xyz.x * target_xyz.x + target_xyz.y * target_xyz.y + target_xyz.z * target_xyz.z) / 100.0; // 米
    
    // 弹道补偿使用 PnP 提供的 3D 坐标
    double x_target = sqrt(target_xyz.x * target_xyz.x + target_xyz.z * target_xyz.z) / 100.0; // 水平距离
    double y_target = -target_xyz.y / 100.0; // 垂直高度差
    double pitch_comp = BallisticSolver::get_pitch(x_target, y_target, MUZZLE_VELOCITY, BALLISTIC_K);

    // 计算是否满足开火条件 (例如残差小于 1.5 度)
    can_fire = AutoTrigger::should_fire(*this, MUZZLE_VELOCITY, yaw, pitch_comp, 0.5);

    return sendTarget(serial, yaw, shoot_delay, can_fire);
}

