#include <eqpalg/utility/fft_get.h>
#include <fftw3.h>

namespace data_process {

Matrix<double, Dynamic, 2> fft_get(Matrix<double, Dynamic, 1> singles,
                                   double fs) {
  int l_singles = singles.rows();
  double f_step = fs / double(l_singles);  //频率分辨率
  Matrix<double, Dynamic, 2> fft_result;
  fft_result.setZero(l_singles / 2, 2);
  fftw_complex *in, *out;
  fftw_plan p;
  in = (fftw_complex*)fftw_malloc(sizeof(fftw_complex) * l_singles);
  out = (fftw_complex*)fftw_malloc(sizeof(fftw_complex) * l_singles);
  for (int i = 0; i < l_singles; i++) {
    in[i][0] = singles(i);
    in[i][1] = 0;
  }
  p = fftw_plan_dft_1d(l_singles, in, out, FFTW_FORWARD, FFTW_ESTIMATE);
  fftw_execute(p); /* repeat as needed */
  for (int i = 0; i < l_singles / 2; i++) {
    fft_result(i, 0) = i * f_step;
    fft_result(i, 1) =
        2 * sqrt(out[i][0] * out[i][0] + out[i][1] * out[i][1]) / l_singles;
  }
  fftw_destroy_plan(p);
  fftw_free(in);
  fftw_free(out);

  return fft_result;
}
}  // namespace data_process