#include <ros/ros.h>
#include "shiyan2/FFT.h"
#include <complex>
#include <cmath>

// 计算FFT的幅度和相位谱
void computeFFT(const std::vector<double>& input, 
                std::vector<double>& magnitude,
                std::vector<double>& phase) {
    const int N = input.size();
    magnitude.resize(N/2+1);
    phase.resize(N/2+1);
    
    // 简单DFT实现 (实际应用中可使用FFTW等库)
    for (int k = 0; k <= N/2; k++) {
        std::complex<double> sum(0.0, 0.0);
        for (int n = 0; n < N; n++) {
            double angle = -2 * M_PI * k * n / N;
            sum += input[n] * std::complex<double>(cos(angle), sin(angle));
        }
        magnitude[k] = std::abs(sum);
        phase[k] = std::arg(sum);
    }
}

// FFT服务回调函数
bool fftCallback(shiyan2::FFT::Request &req,
                 shiyan2::FFT::Response &res) {
    // 确保数据长度匹配
    if (req.data_x.size() != req.window_size || 
        req.data_y.size() != req.window_size) {
        ROS_ERROR("Data size does not match window size!");
        return false;
    }
    
    // 计算x坐标频谱
    computeFFT(req.data_x, res.magnitude_x, res.phase_x);
    
    // 计算y坐标频谱
    computeFFT(req.data_y, res.magnitude_y, res.phase_y);
    
    ROS_INFO("FFT service completed for %d points", req.window_size);
    return true;
}

int main(int argc, char** argv) {
    ros::init(argc, argv, "nodeC");
    ros::NodeHandle nh;
    
    // 创建FFT服务
    ros::ServiceServer service = nh.advertiseService("fft_service", fftCallback);
    ROS_INFO("FFT Service is ready.");
    
    ros::spin();
    return 0;
}