mock-gr/algorithms/fun1_false_target_distance.cpp

//
// File: false_target_distance.cpp
//
// MATLAB Coder version            : 5.2
// C/C++ source code generated on  : 11-Mar-2023 10:38:07
//

// Include Files
#include "fun1_false_target_distance.h"
#include "colon.h"
#include "fun1_fft.h"
#include "fun1_fftshift.h"
#include "fun1_ifft.h"
#include "rt_nonfinite.h"
#include "coder_array.h"
#include <cmath>
#include <iostream>
using namespace std;
// 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;
}

//
// S_orignal开始
//
// Arguments    : double fd
//                double N
//                double ts
//                double Tr
//                double t_r
//                double tau
//                double fo
//                double k
//                double A
//                double dlt_t
//                double R
//                double fs
//                coder::array<double, 2U> &S
//                coder::array<double, 2U> &S1
//                coder::array<double, 2U> &SS
//                coder::array<double, 2U> &S_disturb
//                coder::array<double, 2U> &S3
// Return Type  : void
//
void false_target_distance(double fd, double N, double ts, double Tr,
	double t_r, double tau, double fo, double k,
	double A, double dlt_t, double R, double fs,
	coder::array<double, 2U> &S,
	coder::array<double, 2U> &S1,
	coder::array<double, 2U> &SS,
	coder::array<double, 2U> &S_disturb,
	coder::array<double, 2U> &S3)
{
	coder::array<creal_T, 2U> b_x;
	coder::array<creal_T, 2U> r;
	coder::array<creal_T, 2U> x;
	coder::array<double, 2U> rect;
	coder::array<double, 2U> t;
	coder::array<signed char, 2U> Signal_Radar;
	double a_tmp;
	double b_a_tmp;
	double b_b_tmp;
	double b_tmp;
	double xtmp;
	int ibtile;
	int jcol;
	int jtilecol;
	int ncols;
	int ntilecols;
//	fd = 200 / 3e2;
//	N = 1;
//	ts = 1 / 1e7;
//	Tr = 5e-4;
//	t_r = 2 / 1e4;
//	tau = 30e-6;
//	fo = 1e6;
//	k = 1e12 / 6;
//	A = 1;
//	dlt_t = 25e-6;
//	R = 30e3;
//	fs = 1e7;
	b_tmp = Tr - ts;
	if (std::isnan(ts) || std::isnan(b_tmp)) {
		t.set_size(1, 1);
		t[0] = rtNaN;
	}
	else if ((ts == 0.0) || ((0.0 < b_tmp) && (ts < 0.0)) ||
		((b_tmp < 0.0) && (ts > 0.0))) {
		t.set_size(1, 0);
	}
	else if (std::isinf(b_tmp) && (std::isinf(ts) || (0.0 == b_tmp))) {
		t.set_size(1, 1);
		t[0] = rtNaN;
	}
	else if (std::isinf(ts)) {
		t.set_size(1, 1);
		t[0] = 0.0;
	}
	else if (std::floor(ts) == ts) {
		ibtile = static_cast<int>(std::floor(b_tmp / ts));
		t.set_size(1, ibtile + 1);
		for (ntilecols = 0; ntilecols <= ibtile; ntilecols++) {
			t[ntilecols] = ts * static_cast<double>(ntilecols);
		}
	}
	else {
		coder::eml_float_colon(ts, b_tmp, t);
	}
	// 以ts为间隔产生Tr/ts个抽样
	rect.set_size(1, t.size(1));
	ibtile = t.size(1);
	for (ntilecols = 0; ntilecols < ibtile; ntilecols++) {
		rect[ntilecols] = 0.0;
	}
	ntilecols = t.size(1);
	for (ncols = 0; ncols < ntilecols; ncols++) {
		xtmp = t[ncols];
		if ((xtmp >= t_r) && (xtmp <= tau + t_r)) {
			rect[ncols] = 1.0;
		}
	}
	b_b_tmp = N * Tr - ts;
	if (std::isnan(ts) || std::isnan(b_b_tmp)) {
		t.set_size(1, 1);
		t[0] = rtNaN;
	}
	else if ((ts == 0.0) || ((0.0 < b_b_tmp) && (ts < 0.0)) ||
		((b_b_tmp < 0.0) && (ts > 0.0))) {
		t.set_size(1, 0);
	}
	else if (std::isinf(b_b_tmp) && (std::isinf(ts) || (0.0 == b_b_tmp))) {
		t.set_size(1, 1);
		t[0] = rtNaN;
	}
	else if (std::isinf(ts)) {
		t.set_size(1, 1);
		t[0] = 0.0;
	}
	else if (std::floor(ts) == ts) {
		ibtile = static_cast<int>(std::floor(b_b_tmp / ts));
		t.set_size(1, ibtile + 1);
		for (ntilecols = 0; ntilecols <= ibtile; ntilecols++) {
			t[ntilecols] = ts * static_cast<double>(ntilecols);
		}
	}
	else {
		coder::eml_float_colon(ts, b_b_tmp, t);
	}
	// 以ts为间隔产生N个脉冲信号的抽样间隔
	Signal_Radar.set_size(1, rect.size(1) * static_cast<int>(N));
	ncols = rect.size(1);
	ntilecols = static_cast<int>(N);
	for (jtilecol = 0; jtilecol < ntilecols; jtilecol++) {
		ibtile = jtilecol * ncols;
		for (jcol = 0; jcol < ncols; jcol++) {
			Signal_Radar[ibtile + jcol] = static_cast<signed char>(rect[jcol]);
		}
	}
	//  目标回波信号，重复N个脉冲串
	a_tmp = 6.2831853071795862 * (fo + fd);
	S.set_size(1, t.size(1));
	ibtile = t.size(1);
	for (ntilecols = 0; ntilecols < ibtile; ntilecols++) {
		S[ntilecols] = t[ntilecols] - t_r;
	}
	b_a_tmp = 3.1415926535897931 * k;
	rect.set_size(1, S.size(1));
	ncols = S.size(1);
	for (ntilecols = 0; ntilecols < ncols; ntilecols++) {
		xtmp = S[ntilecols];
		rect[ntilecols] = xtmp * xtmp;
	}
	S.set_size(1, S.size(1));
	ibtile = S.size(1) - 1;
	for (ntilecols = 0; ntilecols <= ibtile; ntilecols++) {
		S[ntilecols] = a_tmp * S[ntilecols] + b_a_tmp * rect[ntilecols];
	}
	ncols = S.size(1);
	for (ntilecols = 0; ntilecols < ncols; ntilecols++) {
		S[ntilecols] = std::cos(S[ntilecols]);
	}
	S.set_size(1, Signal_Radar.size(1));
	ibtile = Signal_Radar.size(1) - 1;
	for (ntilecols = 0; ntilecols <= ibtile; ntilecols++) {
		S[ntilecols] = static_cast<double>(Signal_Radar[ntilecols]) * S[ntilecols];
	//	printf("S--%f--%d\n", S[ntilecols], ntilecols);
	}
	SS.set_size(1, S.size(1));
	ibtile = S.size(1);
	for (ntilecols = 0; ntilecols < ibtile; ntilecols++) {
		SS[ntilecols] = S[ntilecols];
	}
	// S_orignal结束
	// S_match(s)开始
	if (std::isnan(ts) || std::isnan(b_tmp)) {
		t.set_size(t.size(0), 1);
	}
	else if ((ts == 0.0) || ((0.0 < b_tmp) && (ts < 0.0)) ||
		((b_tmp < 0.0) && (ts > 0.0))) {
		t.set_size(t.size(0), 0);
	}
	else if (std::isinf(b_tmp) && (std::isinf(ts) || (0.0 == b_tmp))) {
		t.set_size(t.size(0), 1);
	}
	else if (std::isinf(ts)) {
		t.set_size(t.size(0), 1);
	}
	else if (std::floor(ts) == ts) {
		t.set_size(t.size(0), static_cast<int>(std::floor(b_tmp / ts)) + 1);
	}
	else {
		coder::eml_float_colon(ts, b_tmp, rect);
		t.set_size(t.size(0), rect.size(1));
	}
	// 多少个采样点
	if (1 > t.size(1)) {
		ibtile = 0;
	}
	else {
		ibtile = t.size(1);
	}
	S.set_size(S.size(0), ibtile);
	//  单脉冲目标回波信号
	// 脉冲压缩 ：时域翻折取共轭在进行线性卷积
	coder::fft(S, x);
	rect.set_size(1, ibtile);
	for (ntilecols = 0; ntilecols < ibtile; ntilecols++) {
		rect[ntilecols] = S[ntilecols];
	}
	ncols = ibtile >> 1;
	for (jtilecol = 0; jtilecol < ncols; jtilecol++) {
		ntilecols = (ibtile - jtilecol) - 1;
		xtmp = rect[jtilecol];
		rect[jtilecol] = rect[ntilecols];
		rect[ntilecols] = xtmp;
	}
	coder::fft(rect, r);
	b_x.set_size(1, x.size(1));
	ibtile = x.size(1);
	for (ntilecols = 0; ntilecols < ibtile; ntilecols++) {
		b_x[ntilecols].re =
			x[ntilecols].re * r[ntilecols].re - x[ntilecols].im * r[ntilecols].im;
		b_x[ntilecols].im =
			x[ntilecols].re * r[ntilecols].im + x[ntilecols].im * r[ntilecols].re;
	}
    coder::ifft_1(b_x, x);
	coder::fftshift(x);
	ncols = x.size(1);
	S1.set_size(1, x.size(1));
	for (ntilecols = 0; ntilecols < ncols; ntilecols++) {
		S1[ntilecols] = rt_hypotd_snf(x[ntilecols].re, x[ntilecols].im);
	//	printf("S1--%f--%d\n", S1[ntilecols], ntilecols);
	}
	// S_match(s)结束
	// 干扰信号1开始
	SS.set_size(1, SS.size(1));
	ibtile = SS.size(1) - 1;
	for (ntilecols = 0; ntilecols <= ibtile; ntilecols++) {
		SS[ntilecols] = A * SS[ntilecols];
	//	printf("SS--%f--%d\n", SS[ntilecols], ntilecols);
	}
	// 干扰信号1结束
	// 干扰信号2开始
	if (std::isnan(ts) || std::isnan(b_tmp)) {
		t.set_size(1, 1);
		t[0] = rtNaN;
	}
	else if ((ts == 0.0) || ((0.0 < b_tmp) && (ts < 0.0)) ||
		((b_tmp < 0.0) && (ts > 0.0))) {
		t.set_size(1, 0);
	}
	else if (std::isinf(b_tmp) && (std::isinf(ts) || (0.0 == b_tmp))) {
		t.set_size(1, 1);
		t[0] = rtNaN;
	}
	else if (std::isinf(ts)) {
		t.set_size(1, 1);
		t[0] = 0.0;
	}
	else if (std::floor(ts) == ts) {
		ibtile = static_cast<int>(std::floor(b_tmp / ts));
		t.set_size(1, ibtile + 1);
		for (ntilecols = 0; ntilecols <= ibtile; ntilecols++) {
			t[ntilecols] = ts * static_cast<double>(ntilecols);
		}
	}
	else {
		coder::eml_float_colon(ts, b_tmp, t);
	}
	// 以ts为间隔产生Tr/ts个抽样
	rect.set_size(1, t.size(1));
	ibtile = t.size(1);
	for (ntilecols = 0; ntilecols < ibtile; ntilecols++) {
		rect[ntilecols] = 0.0;
	}
	ntilecols = t.size(1);
	for (ncols = 0; ncols < ntilecols; ncols++) {
		xtmp = t[ncols];
		if ((xtmp >= t_r + dlt_t) && (xtmp <= (tau + t_r) + dlt_t)) {
			rect[ncols] = 1.0;
		}
	}
	if (std::isnan(ts) || std::isnan(b_b_tmp)) {
		t.set_size(1, 1);
		t[0] = rtNaN;
	}
	else if ((ts == 0.0) || ((0.0 < b_b_tmp) && (ts < 0.0)) ||
		((b_b_tmp < 0.0) && (ts > 0.0))) {
		t.set_size(1, 0);
	}
	else if (std::isinf(b_b_tmp) && (std::isinf(ts) || (0.0 == b_b_tmp))) {
		t.set_size(1, 1);
		t[0] = rtNaN;
	}
	else if (std::isinf(ts)) {
		t.set_size(1, 1);
		t[0] = 0.0;
	}
	else if (std::floor(ts) == ts) {
		ibtile = static_cast<int>(std::floor(b_b_tmp / ts));
		t.set_size(1, ibtile + 1);
		for (ntilecols = 0; ntilecols <= ibtile; ntilecols++) {
			t[ntilecols] = ts * static_cast<double>(ntilecols);
		}
	}
	else {
		coder::eml_float_colon(ts, b_b_tmp, t);
	}
	// 以ts为间隔产生N个脉冲信号的抽样间隔
	Signal_Radar.set_size(1, rect.size(1) * static_cast<int>(N));
	ncols = rect.size(1);
	ntilecols = static_cast<int>(N);
	for (jtilecol = 0; jtilecol < ntilecols; jtilecol++) {
		ibtile = jtilecol * ncols;
		for (jcol = 0; jcol < ncols; jcol++) {
			Signal_Radar[ibtile + jcol] = static_cast<signed char>(rect[jcol]);
		}
	}
	//  目标回波信号，重复N个脉冲串
	S_disturb.set_size(1, t.size(1));
	ibtile = t.size(1);
	for (ntilecols = 0; ntilecols < ibtile; ntilecols++) {
		S_disturb[ntilecols] = (t[ntilecols] - t_r) - dlt_t;
	}
	rect.set_size(1, S_disturb.size(1));
	ncols = S_disturb.size(1);
	for (ntilecols = 0; ntilecols < ncols; ntilecols++) {
		xtmp = S_disturb[ntilecols];
		rect[ntilecols] = xtmp * xtmp;
	}
	S_disturb.set_size(1, S_disturb.size(1));
	ibtile = S_disturb.size(1) - 1;
	for (ntilecols = 0; ntilecols <= ibtile; ntilecols++) {
		S_disturb[ntilecols] =
			a_tmp * S_disturb[ntilecols] + b_a_tmp * rect[ntilecols];
	}
	ncols = S_disturb.size(1);
	for (ntilecols = 0; ntilecols < ncols; ntilecols++) {
		S_disturb[ntilecols] = std::cos(S_disturb[ntilecols]);
	}
	S_disturb.set_size(1, Signal_Radar.size(1));
	ibtile = Signal_Radar.size(1) - 1;
	for (ntilecols = 0; ntilecols <= ibtile; ntilecols++) {
		S_disturb[ntilecols] =
			A * static_cast<double>(Signal_Radar[ntilecols]) * S_disturb[ntilecols];
	//	printf("S_disturb--%f--%d\n", S_disturb[ntilecols], ntilecols);
	}
	// 干扰信号2结束
	// 假距离
	if (std::isnan(ts) || std::isnan(b_tmp)) {
		t.set_size(t.size(0), 1);
	}
	else if ((ts == 0.0) || ((0.0 < b_tmp) && (ts < 0.0)) ||
		((b_tmp < 0.0) && (ts > 0.0))) {
		t.set_size(t.size(0), 0);
	}
	else if (std::isinf(b_tmp) && (std::isinf(ts) || (0.0 == b_tmp))) {
		t.set_size(t.size(0), 1);
	}
	else if (std::isinf(ts)) {
		t.set_size(t.size(0), 1);
	}
	else if (std::floor(ts) == ts) {
		t.set_size(t.size(0), static_cast<int>(std::floor(b_tmp / ts)) + 1);
	}
	else {
		coder::eml_float_colon(ts, b_tmp, rect);
		t.set_size(t.size(0), rect.size(1));
	}
	xtmp = std::trunc(2.0 * R / 3.0E+8 * fs);
	S3.set_size(1, static_cast<int>(static_cast<double>(t.size(1)) + xtmp));
	ibtile = static_cast<int>(static_cast<double>(t.size(1)) + xtmp);
	for (ntilecols = 0; ntilecols < ibtile; ntilecols++) {
		S3[ntilecols] = 0.0;
	}
	if (xtmp + 1.0 > static_cast<double>(t.size(1)) + xtmp) {
		ntilecols = 1;
	}
	else {
		ntilecols = static_cast<int>(xtmp + 1.0);
	}
	ibtile = S1.size(1);
	for (ncols = 0; ncols < ibtile; ncols++) {
		S3[(ntilecols + ncols) - 1] = S1[ncols] * 1.5;
//		printf("S3--%f--%d\n", S3[ncols], ncols);
	}
}


//int main()
//{
//    coder::array<double, 2U> S;
//    coder::array<double, 2U> S1;
//    coder::array<double, 2U> S3;
//    coder::array<double, 2U> SS;
//    coder::array<double, 2U> S_disturb;
//    double fd = 200 / 3e2;
//    double N = 1;
//    double ts = 1 / 1e7;
//    double Tr = 5e-4;
//    double t_r = 2 / 1e4;
//    double tau = 30e-6;
//    double fo = 1e6;
//    double k = 1e12 / 6;
//    double A = 1;
//    double dlt_t = 25e-6;
//    double R = 30e3;
//    double fs = 1e7;
//
//    false_target_distance(fd, N, ts, Tr, t_r, tau, fo, k, A, dlt_t, R, fs, S, S1, SS,S_disturb, S3);
//    //输出S
//    cout << "S=" <<endl;
//    for (int i = 0; i < S.size(1); i++) {
//        std::cout << S[i] << " ";
//    }
//    cout << endl;
//    //输出S1
//    cout << "S1=" <<endl;
//    for (int i = 0; i < S1.size(1); i++) {
//        std::cout << S1[i] << " ";
//    }
//    cout << endl;
//    //输出SS
//    cout << "SS=" <<endl;
//    cout << SS.size(1)<<endl;
//    for (int i = 0; i < SS.size(1); i++) {
//        std::cout << SS[i] << " ";
//    }
//    cout << endl;
//    //输出S_disturb
//    cout << "S_disturb=" <<endl;
//    for (int i = 0; i < S_disturb.size(1); i++) {
//        std::cout << S_disturb[i] << " ";
//    }
//    cout << endl;
//    //输出S3
//    cout << "S3=" <<endl;
//    for (int i = 0; i < S3.size(1); i++) {
//        std::cout << S3[i] << " ";
//    }
//}