mock-gr/algorithms/fun8_2_FMjamming_H.cpp

//
// File: FMjamming_H.cpp
//
// MATLAB Coder version            : 5.2
// C/C++ source code generated on  : 10-Mar-2023 11:08:29
//

// Include Files
#include "fun8_2_FMjamming_H.h"
#include "fun8_2_FMjamming_H_data.h"
#include "fun8_2_ifft.h"
#include "fun8_2_randn.h"
#include "rt_nonfinite.h"
#include "fun8_2_std.h"
#include "coder_array.h"
#include <cmath>
#include <iostream>
#include "fun8_2_eml_rand_mt19937ar_stateful.h"
using namespace std;


void FMjamming_H1_initialize()
{
    eml_rand_mt19937ar_stateful_init_8_2();
    isInitialized_FMjamming_H1 = true;
}

// Function Declarations
static double rt_hypotd_snf(double u0, double u1);

// Function Definitions
//
// Arguments    : double u0
//                double u1
// Return Type  : double
//
static double rt_hypotd_snf(double u0, double u1)
{
  double a;
  double y;
  a = std::abs(u0);
  y = std::abs(u1);
  if (a < y) {
    a /= y;
    y *= std::sqrt(a * a + 1.0);
  } else if (a > y) {
    y /= a;
    y = a * std::sqrt(y * y + 1.0);
  } else if (!std::isnan(y)) {
    y = a * 1.4142135623730951;
  }
  return y;
}

//
// Arguments    : double N
//                double B_n
//                double f_s
//                double T_s
//                double m_fe
//                double A
//                double f_0
//                coder::array<double, 2U> &s
//                coder::array<double, 2U> &s_0
// Return Type  : void
//
void FMjamming_H(double N, double B_n, double f_s, double T_s, double m_fe,
                 double A, double f_0, coder::array<double, 2U> &s,
                 coder::array<double, 2U> &s_0)
{
  coder::array<creal_T, 2U> S;
  coder::array<creal_T, 2U> c_s_n;
  coder::array<double, 2U> b;
  coder::array<double, 2U> b_s_n;
  coder::array<double, 2U> sum;
  coder::array<double, 1U> absdiff;
  double s_n[2];
  double N_n;
  double bsum_im;
  double bsum_re;
  double xbar_im;
  int bsum_re_tmp;
  int firstBlockLength;
  int k;
  int loop_ub_tmp;
  int n;
  int xblockoffset;
  if (!isInitialized_FMjamming_H1) {
    FMjamming_H1_initialize();
  }
  if (std::isnan(N)) {
    s.set_size(1, 1);
    s[0] = rtNaN;
  } else if (N < 1.0) {
    s.set_size(s.size(0), 0);
  } else if (std::isinf(N) && (1.0 == N)) {
    s.set_size(1, 1);
    s[0] = rtNaN;
  } else {
    firstBlockLength = static_cast<int>(std::floor(N - 1.0));
    s.set_size(1, firstBlockLength + 1);
    for (bsum_re_tmp = 0; bsum_re_tmp <= firstBlockLength; bsum_re_tmp++) {
      s[bsum_re_tmp] = static_cast<double>(bsum_re_tmp) + 1.0;
    }
  }
  N_n = std::round(N * (B_n / (2.0 * f_s)));
  loop_ub_tmp = static_cast<int>(N);
  s_n[0] = 1.0;
  s_n[1] = static_cast<int>(N);
    coder::randn_8_2(s_n, sum);
  b_s_n.set_size(1, loop_ub_tmp);
  for (bsum_re_tmp = 0; bsum_re_tmp < loop_ub_tmp; bsum_re_tmp++) {
    b_s_n[bsum_re_tmp] = sum[bsum_re_tmp];
  }
  s_n[0] = 1.0;
  s_n[1] = static_cast<int>(N);
    coder::randn_8_2(s_n, sum);
  b.set_size(1, loop_ub_tmp);
  for (bsum_re_tmp = 0; bsum_re_tmp < loop_ub_tmp; bsum_re_tmp++) {
    b[bsum_re_tmp] = sum[bsum_re_tmp];
  }
  S.set_size(1, b_s_n.size(1));
  firstBlockLength = b_s_n.size(1);
  for (bsum_re_tmp = 0; bsum_re_tmp < firstBlockLength; bsum_re_tmp++) {
    S[bsum_re_tmp].re = b_s_n[bsum_re_tmp] + 0.0 * b[bsum_re_tmp];
    S[bsum_re_tmp].im = b[bsum_re_tmp];
  }
  bsum_re_tmp = static_cast<int>(((N - N_n) - 1.0) + (1.0 - N_n));
  for (xblockoffset = 0; xblockoffset < bsum_re_tmp; xblockoffset++) {
    firstBlockLength =
        static_cast<int>(N_n + static_cast<double>(xblockoffset)) - 1;
    S[firstBlockLength].re = 0.0;
    S[firstBlockLength].im = 0.0;
  }
    coder::ifft_8_2(S, c_s_n);
  n = c_s_n.size(1);
  if (c_s_n.size(1) == 0) {
    xbar_im = rtNaN;
  } else if (c_s_n.size(1) == 1) {
    if ((!std::isinf(c_s_n[0].re)) && (!std::isinf(c_s_n[0].im)) &&
        ((!std::isnan(c_s_n[0].re)) && (!std::isnan(c_s_n[0].im)))) {
      xbar_im = 0.0;
    } else {
      xbar_im = rtNaN;
    }
  } else {
    int lastBlockLength;
    int nblocks;
    if (c_s_n.size(1) <= 1024) {
      firstBlockLength = c_s_n.size(1);
      lastBlockLength = 0;
      nblocks = 1;
    } else {
      firstBlockLength = 1024;
      nblocks = c_s_n.size(1) / 1024;
      lastBlockLength = c_s_n.size(1) - (nblocks << 10);
      if (lastBlockLength > 0) {
        nblocks++;
      } else {
        lastBlockLength = 1024;
      }
    }
    N_n = c_s_n[0].re;
    xbar_im = c_s_n[0].im;
    for (k = 2; k <= firstBlockLength; k++) {
      N_n += c_s_n[k - 1].re;
      xbar_im += c_s_n[k - 1].im;
    }
    for (int ib{2}; ib <= nblocks; ib++) {
      xblockoffset = (ib - 1) << 10;
      bsum_re = c_s_n[xblockoffset].re;
      bsum_im = c_s_n[xblockoffset].im;
      if (ib == nblocks) {
        firstBlockLength = lastBlockLength;
      } else {
        firstBlockLength = 1024;
      }
      for (k = 2; k <= firstBlockLength; k++) {
        bsum_re_tmp = (xblockoffset + k) - 1;
        bsum_re += c_s_n[bsum_re_tmp].re;
        bsum_im += c_s_n[bsum_re_tmp].im;
      }
      N_n += bsum_re;
      xbar_im += bsum_im;
    }
    if (xbar_im == 0.0) {
      bsum_im = N_n / static_cast<double>(c_s_n.size(1));
      N_n = 0.0;
    } else if (N_n == 0.0) {
      bsum_im = 0.0;
      N_n = xbar_im / static_cast<double>(c_s_n.size(1));
    } else {
      bsum_im = N_n / static_cast<double>(c_s_n.size(1));
      N_n = xbar_im / static_cast<double>(c_s_n.size(1));
    }
    absdiff.set_size(c_s_n.size(1));
    for (k = 0; k < n; k++) {
      absdiff[k] = rt_hypotd_snf(c_s_n[k].re - bsum_im, c_s_n[k].im - N_n);
    }
    n = c_s_n.size(1);
    xbar_im = 0.0;
    N_n = 3.3121686421112381E-170;
    for (k = 0; k < n; k++) {
      if (absdiff[k] > N_n) {
        bsum_re = N_n / absdiff[k];
        xbar_im = xbar_im * bsum_re * bsum_re + 1.0;
        N_n = absdiff[k];
      } else {
        bsum_re = absdiff[k] / N_n;
        xbar_im += bsum_re * bsum_re;
      }
    }
    xbar_im = N_n * std::sqrt(xbar_im);
    xbar_im /= std::sqrt(static_cast<double>(c_s_n.size(1)) - 1.0);
  }
  c_s_n.set_size(1, c_s_n.size(1));
  firstBlockLength = c_s_n.size(1) - 1;
  for (bsum_re_tmp = 0; bsum_re_tmp <= firstBlockLength; bsum_re_tmp++) {
    N_n = c_s_n[bsum_re_tmp].re;
    bsum_re = c_s_n[bsum_re_tmp].im;
    if (bsum_re == 0.0) {
      bsum_im = N_n / xbar_im;
      N_n = 0.0;
    } else if (N_n == 0.0) {
      bsum_im = 0.0;
      N_n = bsum_re / xbar_im;
    } else {
      bsum_im = N_n / xbar_im;
      N_n = bsum_re / xbar_im;
    }
    c_s_n[bsum_re_tmp].re = bsum_im;
    c_s_n[bsum_re_tmp].im = N_n;
  }
  b_s_n.set_size(1, c_s_n.size(1));
  firstBlockLength = c_s_n.size(1);
  for (bsum_re_tmp = 0; bsum_re_tmp < firstBlockLength; bsum_re_tmp++) {
    b_s_n[bsum_re_tmp] = c_s_n[bsum_re_tmp].re;
  }
  sum.set_size(1, loop_ub_tmp);
  for (bsum_re_tmp = 0; bsum_re_tmp < loop_ub_tmp; bsum_re_tmp++) {
    sum[bsum_re_tmp] = 0.0;
  }
  bsum_re_tmp = static_cast<int>(N - 1.0);
  for (xblockoffset = 0; xblockoffset < bsum_re_tmp; xblockoffset++) {
    sum[xblockoffset + 1] = b_s_n[xblockoffset] + sum[xblockoffset];
  }
  if (m_fe >= 0.75) {
    N_n = B_n / 2.35 / coder::b_std_8_2(b_s_n);
  } else {
    N_n = m_fe * (B_n / (3.1415926535897931 * (m_fe * m_fe))) /
            coder::b_std_8_2(b_s_n);
  }
  bsum_re = 6.2831853071795862 * f_0;
  s.set_size(1, s.size(1));
  firstBlockLength = s.size(1) - 1;
  for (bsum_re_tmp = 0; bsum_re_tmp <= firstBlockLength; bsum_re_tmp++) {
    s[bsum_re_tmp] = bsum_re * s[bsum_re_tmp] * T_s;
  }
  s_0.set_size(1, s.size(1));
  firstBlockLength = s.size(1);
  for (bsum_re_tmp = 0; bsum_re_tmp < firstBlockLength; bsum_re_tmp++) {
    s_0[bsum_re_tmp] = s[bsum_re_tmp];
  }
  firstBlockLength = s.size(1);
  for (k = 0; k < firstBlockLength; k++) {
    s_0[k] = std::cos(s_0[k]);
  }
  s_0.set_size(1, s_0.size(1));
  firstBlockLength = s_0.size(1) - 1;
  for (bsum_re_tmp = 0; bsum_re_tmp <= firstBlockLength; bsum_re_tmp++) {
    s_0[bsum_re_tmp] = A * s_0[bsum_re_tmp];
  }
  bsum_re = 6.2831853071795862 * N_n;
  s.set_size(1, s.size(1));
  firstBlockLength = s.size(1) - 1;
  for (bsum_re_tmp = 0; bsum_re_tmp <= firstBlockLength; bsum_re_tmp++) {
    s[bsum_re_tmp] = s[bsum_re_tmp] + bsum_re * (sum[bsum_re_tmp] * T_s);
  }
  firstBlockLength = s.size(1);
  for (k = 0; k < firstBlockLength; k++) {
    s[k] = std::cos(s[k]);
  }
  s.set_size(1, s.size(1));
  firstBlockLength = s.size(1) - 1;
  for (bsum_re_tmp = 0; bsum_re_tmp <= firstBlockLength; bsum_re_tmp++) {
    s[bsum_re_tmp] = A * s[bsum_re_tmp];
  }
}

//int main()
//{
//    coder::array<double, 2U> s;
//    coder::array<double, 2U> s_0;
//    double N=10000;
//    double B_n=2e6;
//    double f_s=100e6;
//    double T_s=100e-6;
//    double m_fe=1;
//    double A=1;
//    double f_0=10e6;
//    FMjamming_H(N, B_n, f_s, T_s, m_fe, A, f_0, s, s_0);
//    // 输出s
//    cout << "s=" <<endl;
//    for (int i = 0; i < s.size(1); i++) {
//        std::cout << s[i] << " ";
//    }
//    cout << endl;
//
////    // 输出s_0
////    cout << "s_0=" <<endl;
////    for (int i = 0; i < s_0.size(1); i++) {
////        std::cout << s_0[i] << " ";
////    }
////    cout << endl;
//    return 0;
//}