//
// Created by xixiliadorabarry on 1/24/19.
//
#include "energy/energy.h"
#include <iostream>
#include "log.h"
#include "config/setconfig.h"

using namespace std;

// amadeus_26 协议: 15字节帧
// | SOF(0xBB 0x77) | x_move(2B) | y_move(2B) | yaw(2B) | pitch(2B) | feed(2B) | key(1B) | crc8(1B) | EOF(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;

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

//----------------------------------------------------------------------------------------------------------------------
// 此函数用于发送能量机关数据
// ---------------------------------------------------------------------------------------------------------------------
void Energy::sendEnergy() {
    sum_yaw += yaw_rotation;
    sum_pitch += pitch_rotation;
    float yaw_I_component = YAW_AIM_KI * sum_yaw;
    float pitch_I_component = PITCH_AIM_KI * sum_pitch;
    MINMAX(yaw_I_component, -2, 2);
    MINMAX(pitch_I_component, -2, 2);

    double tmp_yaw = yaw_rotation;
    double tmp_pitch = pitch_rotation;
    yaw_rotation = YAW_AIM_KP * yaw_rotation + YAW_AIM_KI * sum_yaw +
                   YAW_AIM_KD * (yaw_rotation - last_yaw);
    pitch_rotation = PITCH_AIM_KP * pitch_rotation + PITCH_AIM_KI * sum_pitch +
                     PITCH_AIM_KD * (pitch_rotation - last_pitch);

    last_yaw = tmp_yaw;
    last_pitch = tmp_pitch;

    // 能量机关模式通过 key 字段区分
    // 模式编码: 0=普通跟随, 3=目标切换, 4=猜测模式
    uint8_t mode_key = 0;
    int16_t feed = 0;
    if (shoot) {
        feed = 660;  // 开火
    }

    if (change_target) {
        mode_key = 3;  // 目标切换
    } else if (is_guessing) {
        mode_key = 4;  // 猜测模式
    }

    sendTarget(serial, yaw_rotation, pitch_rotation, feed, mode_key);
}


//----------------------------------------------------------------------------------------------------------------------
// 此函数用于发送数据给主控板 (amadeus_26 协议)
// ---------------------------------------------------------------------------------------------------------------------
void Energy::sendTarget(Serial &serial, float yaw, float pitch, int16_t feed, uint8_t key) {
    // 控制参数
    int16_t x_move = 0;           // 平动左右 [-660, 660]
    int16_t y_move = 0;           // 平动前后 [-660, 660]
    int16_t yaw_val = 0;          // 云台偏航 [-660, 660]
    int16_t pitch_val = 0;        // 云台俯仰 [-660, 660]
    uint8_t left_switch = 3;      // 左拨杆 [1, 3] 2 摩擦轮打开 3 关闭
    uint8_t right_switch = 3;     // 右拨杆 [1, 3] 2 小陀螺 3 关闭

#ifdef WITH_COUNT_FPS
    static auto last_time = time(nullptr);
    static int fps = 0;
    time_t t = time(nullptr);
    if (last_time != t) {
        last_time = t;
        cout << "Energy: fps:" << fps << ", (" << yaw << "," << pitch << ", feed:" << feed << ", key:" << (int)key << ")" << endl;
        curr_fps = fps;
        fps = 0;
    }
    fps += 1;
#endif

    // 将角度 (-3, 3) 度 转换为协议范围 [-660, 660]
    // 映射公式: output = input * (660 / 3) = input * 220
    yaw_val = static_cast<int16_t>(yaw * 220);
    pitch_val = static_cast<int16_t>(pitch * 220);
    MINMAX(yaw_val, -660, 660);
    MINMAX(pitch_val, -660, 660);

    // 构建数据帧 - 与 test_pitch_up.cpp 格式一致
    uint8_t frame[FRAME_LENGTH];
    int idx = 0;

    // 帧头
    frame[idx++] = FRAME_HEADER_1;
    frame[idx++] = FRAME_HEADER_2;

    // 平动左右 (2 bytes, int16, 小端序)
    frame[idx++] = x_move & 0xFF;
    frame[idx++] = (x_move >> 8) & 0xFF;

    // 平动前后 (2 bytes, 小端序)
    frame[idx++] = y_move & 0xFF;
    frame[idx++] = (y_move >> 8) & 0xFF;

    // 云台偏航 (2 bytes, 小端序)
    frame[idx++] = yaw_val & 0xFF;
    frame[idx++] = (yaw_val >> 8) & 0xFF;

    // 云台俯仰 (2 bytes, 小端序)
    frame[idx++] = pitch_val & 0xFF;
    frame[idx++] = (pitch_val >> 8) & 0xFF;

    // 拨弹轮 (2 bytes, 小端序)
    frame[idx++] = feed & 0xFF;
    frame[idx++] = (feed >> 8) & 0xFF;

    // 拨杆 (1 byte: 高4位左拨杆，低4位右拨杆)
    // key 参数通过低4位传递能量机关模式信息
    frame[idx++] = (left_switch << 4) | (key & 0x0F);

    // CRC8 (固定为 0xCC)
    frame[idx++] = 0xCC;

    // 帧尾
    frame[idx++] = FRAME_TAIL;

    // 调试输出：打印完整帧内容 - 与 test_pitch_up.cpp 格式一致
    std::string frame_hex;
    char buf[4];
    for (int i = 0; i < FRAME_LENGTH; i++) {
        snprintf(buf, sizeof(buf), "%02X ", frame[i]);
        frame_hex += buf;
    }
    LOGM(STR_CTR(WORD_LIGHT_PURPLE, "[TX](%dB): %s | yaw=%d pitch=%d feed=%d key=%d"), 
         FRAME_LENGTH, frame_hex.c_str(), yaw_val, pitch_val, feed, key);

    serial.WriteData(frame, sizeof(frame));
    send_cnt += 1;
}
