# Interface
# レーダーの信号処理とデータサイエンス
#　演習４：人物データの抽出
#　(c) 米本成人
#   July, 2023

import numpy as np
import matplotlib.pyplot as plt

#タイトルで漢字が使えるようフォントを設定
plt.rcParams['font.family'] = 'Meiryo'

#レーダーのパラメータ設定
MaxRange = 100#最大距離
ViewAngle = 30#m
N=MaxRange*2+2#サンプル数1メモリ1mとして、0からMaxRangeまで
dt = 1/(N-1)#サンプリング周期
M = 16 #アンテナ数
mu, sigma = 0.0, 1 #ノイズの平均値、標準偏差

antenna = np.linspace(1,M,M)#アンテナ番号
angle = np.pi*np.linspace(-ViewAngle/2, ViewAngle/2,M)/180#アンテナの画角設定（radian）
distance = np.linspace(0,N,N)#距離(m)
t = np.linspace(0, 1, N) # 時間 [s]

# 3D描画用のmeshgrid を作成
range_mesh, antenna_mesh = np.meshgrid(distance, antenna)#アンテナ番号と距離に合わせたメッシュデータを作る
range_mesh, angle_mesh = np.meshgrid(distance, angle)#角度と距離に合わせたメッシュデータを作る。
x_mesh = range_mesh * np.sin(angle_mesh)#レーダーからみて横方向の座標をX軸とする
y_mesh = range_mesh * np.cos(angle_mesh)#レーダーから見て距離方向の座標をY軸とする
x1, x2 = np.array_split(x_mesh, 2, 1)
y1, y2 = np.array_split(y_mesh, 2, 1)

#レーダー画像データ作成
#とりあえず空の行列を作る
noisedata = 0*range_mesh
signaldata= 0*range_mesh
#２つの物体からの反射波の周波数、振幅、および到来角を設定。
f1, f2 = 50, 90    # 周波数 [kHz]
a1, a2 = 10, 15        # 振幅
p1, p2 = -6/16, 2/16    # 位相
for m in range(M):
    noisedata[m]=np.random.normal(mu, sigma, N)#ノイズ
    signaldata[m]=a1*np.cos(2*np.pi*(f1*t+p1*m)) + a2*np.sin(2*np.pi*(f2*t+p2*m)) # 信号

timedomaindata = signaldata+noisedata#M個のアンテナの時間波形

#フィルタ作成
theta = np.linspace(0,1,M)#アンテナ毎のポイント
#filan = 0.5-0.5*np.cos(2*np.pi*theta)#空間方向にHanning window
filan = np.linspace(1,1,M)#時間方向の振幅今回は方形窓、
fildis = 0.5-0.5*np.cos(2*np.pi*distance/N)#時間方向にHanning window
f, a = np.meshgrid(fildis, filan) #縦横メッシュデータを作成
filterdata = f*a #アンテナ方向と距離方向の重みを合成

#空間フィルタリング
filtereddata= filterdata*timedomaindata

#   2DFFT　
fftdata = np.fft.fft2(filtereddata) #2次元FFT
absdata = abs(fftdata)#振幅計算
magdata = 20*np.log10(absdata)#dBに変換
posdata, negadata = np.array_split(magdata, 2, 1)#ベースバンドを正の周波数と負の周波数に分割
leftdata, rightdata = np.array_split(posdata, 2)#左右に分割
mapdata = np.vstack([rightdata, leftdata])#タイリングでFFTの折り返しを無くす

#閾値処理
threshold = 50
detecteddata = np.where(mapdata > threshold, mapdata, 0)

fig19 = plt.figure(19)
ax = fig19.add_subplot(projection='3d')#曲面を描画
ax.plot_surface(x1,y1,detecteddata,cmap="jet")
ax.set_title("閾値処理")#グラフタイトルを設定
ax.set_xlabel("x座標(m)",color="black")#軸ラベル
ax.set_ylabel("y座標(m)",color="black")
ax.set_zlabel("信号強度",color="black")
plt.savefig("./Fig19.png") #図の保存



#背景画像データ作成
#とりあえず空の行列を作る
noisedata = 0*range_mesh
signaldata= 0*range_mesh
#２つの物体からの反射波の周波数、振幅、および到来角を設定。
f1, f2 = 50, 90    # 周波数 [kHz]
a1, a2 = 10, 15        # 振幅
p1, p2 = -6/16, 2/16    # 位相
for m in range(M):
    noisedata[m]=np.random.normal(mu, sigma, N)#ノイズ
    signaldata[m]=a1*np.cos(2*np.pi*(f1*t+p1*m)) # 柱の信号

timedomaindata = signaldata+noisedata#M個のアンテナの時間波形

#フィルタ作成
theta = np.linspace(0,1,M)#アンテナ毎のポイント
#filan = 0.5-0.5*np.cos(2*np.pi*theta)#空間方向にHanning window
filan = np.linspace(1,1,M)#時間方向の振幅今回は方形窓、
fildis = 0.5-0.5*np.cos(2*np.pi*distance/N)#時間方向にHanning window
f, a = np.meshgrid(fildis, filan)
filterdata = f*a
filtereddata= filterdata*timedomaindata

fftbdata = np.fft.fft2(filtereddata) #2次元FFTで背景画像を計算
subtracteddata = fftdata-fftbdata #背景画像を引く
absdata = abs(subtracteddata)#振幅計算
magdata = 20*np.log10(absdata+0.1)#dBに変換
posdata, negadata = np.array_split(magdata, 2, 1)#ベースバンドを正の周波数と負の周波数に分割
leftdata, rightdata = np.array_split(posdata, 2)#左右に分割
mapdata = np.vstack([rightdata, leftdata])#タイリングでFFTの折り返しを無くす

fig20 = plt.figure(20)
ax = fig20.add_subplot(projection='3d')
#曲面を描画
ax.plot_surface(x1,y1,mapdata,cmap="jet")
ax.set_title("差分画像")#グラフタイトルを設定
ax.set_xlabel("x座標(m)",color="black")#軸ラベル
ax.set_ylabel("y座標(m)",color="black")
ax.set_zlabel("信号強度",color="black")
plt.savefig("./Fig20.png") #図の保存

plt.show()
