#!/usr/bin/env python3

import rospy
import math, cmath
from Service.srv import FFT, FFTResponse

def computeFFT(input_data):
    N = len(input_data)
    magnitude = [0.0] * (N//2 + 1)    # 存储幅值
    phase = [0.0] * (N//2 + 1)    # 存储相位

    for k in range(N//2 + 1):
        s = 0+0j

        for n in range(N):
            angle = -2 * math.pi * k * n / N    # 角度计算
            s += input_data[n] * complex(math.cos(angle), math.sin(angle))    # 快速傅里叶变换公式
        
        magnitude[k] = abs(s)    # 幅值
        phase[k] = cmath.phase(s)    # 相位
    
    return magnitude, phase

def fft_callback(req):
    if len(req.data_x) != req.window_size or len(req.data_y) != req.window_size:
        rospy.logerr("数据量不符合128点FFT要求")
        return FFTResponse()
    
    mag_x, phase_x = computeFFT(req.data_x)    # 对坐标 x 和 y 分别进行快速傅里叶变换FFT
    mag_y, phase_y = computeFFT(req.data_y)

    res = FFTResponse()
    res.magnitude_x = mag_x    # 坐标 x 的幅度
    res.magnitude_y = mag_y    # 坐标 y 的幅度
    res.phase_x = phase_x    # 坐标 x 的相位
    res.phase_y = phase_y    # 坐标 y 的相位

    rospy.loginfo("FFT 服务完成 %d 个点" % req.window_size)
    return res

if __name__ == '__main__':
    rospy.init_node('Service_S')

    service = rospy.Service('fft_service', FFT, fft_callback)    # ROS 服务

    rospy.loginfo("服务端快速傅里叶变换程序执行成功")
    rospy.spin()