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

#include <armor_finder/armor_finder.h>
#include <config/setconfig.h>
#include <log.h>
#include <string>
#include <cstdio>

// ANGLE_SCALE: 角度单位换算系数。发出的整数 = yaw_deg / ANGLE_SCALE
// 例: 1.0f → 单位为度; 0.01f → 单位为厘度(精度更高)
static constexpr float ANGLE_SCALE = 1.0f;

static bool sendTarget(Serial &serial, double yaw_deg, double pitch_deg, double dist,
                       uint16_t shoot_delay, char type = 'r') {
    // static short x_tmp, y_tmp, z_tmp;
    // uint8_t buff[10];
#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_deg
             << " pitch=" << pitch_deg << " dist=" << dist << endl;
        fps = 0;
    }
    fps += 1;
#endif

    // ---- 新格式: 字符串帧 ----
    // 格式: "s <yaw_int> <pitch_int> <dist> <shoot_delay> e\n"
    int16_t yaw_tmp   = static_cast<int16_t>(yaw_deg   / ANGLE_SCALE);
    int16_t pitch_tmp = static_cast<int16_t>(pitch_deg / ANGLE_SCALE);
    char buf[64];
    std::snprintf(buf, sizeof(buf), "s %d %d %.1f %u e\n",
                  (int)yaw_tmp, (int)pitch_tmp, dist, (unsigned)shoot_delay);
    std::string frame(buf);
    return serial.WriteData(reinterpret_cast<const uint8_t*>(frame.data()), frame.size());


    // ---- 原始二进制打包（已注释）----
    // static short x_tmp, y_tmp, z_tmp;
    // uint8_t buff[10];
    // x_tmp = static_cast<short>(yaw_deg   * (32768 - 1) / 100);
    // y_tmp = static_cast<short>(pitch_deg * (32768 - 1) / 100);
    // z_tmp = static_cast<short>(dist      * (32768 - 1) / 1000);
    // buff[0] = 's';
    // buff[1] = static_cast<char>((x_tmp >> 8) & 0xFF);
    // buff[2] = static_cast<char>((x_tmp >> 0) & 0xFF);
    // buff[3] = static_cast<char>((y_tmp >> 8) & 0xFF);
    // buff[4] = static_cast<char>((y_tmp >> 0) & 0xFF);
    // buff[5] = static_cast<char>((z_tmp >> 8) & 0xFF);
    // buff[6] = static_cast<char>((z_tmp >> 0) & 0xFF);
    // buff[7] = static_cast<char>((shoot_delay >> 8) & 0xFF);
    // buff[8] = static_cast<char>((shoot_delay >> 0) & 0xFF);
    // buff[9] = 'e';
    // return serial.WriteData(buff, sizeof(buff));
}

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
    #define MAX_I 100 // 这里的数值可以根据实际调试情况进一步微调
    sum_yaw += dx;
    sum_pitch += dy;
    sum_yaw = (sum_yaw > MAX_I) ? MAX_I : (sum_yaw < -MAX_I ? -MAX_I : sum_yaw);
    sum_pitch = (sum_pitch > MAX_I) ? MAX_I : (sum_pitch < -MAX_I ? -MAX_I : sum_pitch);

    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 pitch = atan(dy / FOCUS_PIXAL) * 180 / PI;
    double dist = DISTANCE_HEIGHT / rect.height;
    return sendTarget(serial, yaw, -pitch, dist, shoot_delay);
}
