isdefined(Main,:init_func) || include("init.jl")

# Radar Parameters
fc = 12e9 # Hz, carrier frequency
c = physconst("LightSpeed") # m/s, the speed of signal propagation
bw = 100e6 # Hz, signal band
fs = bw # Hz, sampling rate

# Parameters of the receiving and transmitting path
Pt = 10 # W, peak power
Gtx = 40 # dB, gain of the transmitting antenna
Grx = 40 # dB, gain of the receiving antenna
NF = 2.9 # dB, the noise coefficient of the receiving path

# Formation of system objects (CO) of functional radar nodes
antenna = EngeePhased.IsotropicAntennaElement( # WITH an isotropic antenna element
    BackBaffled=false # taking into account the backscattering of the antenna bottom
) 
transmitter = EngeePhased.Transmitter( # CO - transmitter
    Gain=Gtx, # transmitter gain
    PeakPower=Pt # Peak power
)
radiator = EngeePhased.Radiator( # CO - transmitting antenna
    Sensor=antenna, # The bottom of the antenna element
    OperatingFrequency=fc # antenna carrier frequency
)
collector = EngeePhased.Collector( # CO - receiving antenna
    Sensor=antenna, # The bottom of the antenna element
    OperatingFrequency=fc # antenna carrier frequency
)
receiver = EngeePhased.ReceiverPreamp(
    Gain=Grx, # receiver gain
    NoiseFigure=NF, # the noise factor in the path
    SampleRate=fs # sampling rate
)

# Creating a CO - goal
tgtrcs = 1000 # m^2, EPR goals
target = EngeePhased.RadarTarget(
    MeanRCS=tgtrcs, # the average EPR value
    PropagationSpeed=c, # The speed of propagation
    OperatingFrequency=fc # ZS carrier frequency
)

# The model of the goal movement scenario
tgtpos = [80; 40; 110]; # [pos_x,pos_y,pos_z],m is the initial position vector
tgtvel = [50; 40; 0]; # [v_x,v_y,v_z],m/s velocity vector
tgtplatform = EngeePhased.Platform( # CO - model of movement
    InitialPosition=tgtpos,  # initial position vector
    Velocity=tgtvel # velocity vector
) 

# Radar motion scenario model
radarpos = [ # m, initial positions
    0 300 100 200 150; 
    0 50 -200 300 100;
    0 -10 10 5 20
]

# Radar motion scenario model
numRadar = size(radarpos,2) # Number of radars
radarvel = zeros(3,numRadar) # [v_x,v_y,v_z] [m/s], velocity vectors
radarplatform = EngeePhased.Platform( # CO - model of radar movement
    InitialPosition=radarpos,  # initial position vector
    Velocity=radarvel # velocity vector
);

# Base station model
txpos = [-150; -100; 50] # starting position
txvel = [0; 0; 0] # velocity vector
txplatform = EngeePhased.Platform(
    InitialPosition=txpos,
    Velocity=txvel
);

# Formation of a phase-modulated signal
N = 1024 # !!!Number of reports per pulse (fast time counts)
M = 8 # Number of pulses (slow time counts)
numChip = 2^nextpow2(N)-1 # The number of chips in the FKM signal code
tchip = 1/bw # s, the duration of the chip
tWave = numChip * tchip # The modulation period
prf = 1/tWave # Hz, pulse repetition rate

# Configure the phase coded waveform as the maximum length sequence
pmcwWaveform = EngeePhased.PhaseCodedWaveform(
    Code="ZadoffChu", # The signal code
    SampleRate=fs, # sampling rate
    NumChips=numChip, # number of chips
    ChipWidth=tchip, # chip duration
    OutputFormat="Samples", # output format - samples
    NumSamples=numChip  # number of chips per pulse
)
sig_pmcw = pmcwWaveform()*ones(1,M);
println("Dimension of the CC: $(size(sig_pmcw))")

plot_sig_and_spec(sig_pmcw[:,1];fs=fs,name_sig = "FKM")

 # the channel model for the base station
txchannel = EngeePhased.FreeSpace(
    PropagationSpeed=c,
    OperatingFrequency=fc,
    SampleRate=fs,
    TwoWayPropagation=false
)

# A similar model for the target-RADAR channel
rxchannel = deepcopy(txchannel);

# Allocation of memory for the reflected signal for all radars
X = zeros(ComplexF64,size(sig_pmcw)...,numRadar)

# Base station signal
txsig = transmitter.(sig_pmcw)

for rad_i in 1:numRadar
    # Memory allocation for calculating the response from the I-th radar
    x = zeros(ComplexF64,size(sig_pmcw)...)

    # Scenario Model
    for m in 1:M
        # Updates to the position of the base station, radar, and target
        tx_pos,tx_vel = txplatform(tWave)
        radar_pos,radar_vel = radarplatform(tWave)
        tgt_pos,tgt_vel = tgtplatform(tWave)

        # Calculating the viewing angle of the target base station
        _,txang = rangeangle(tgt_pos,tx_pos) 

        # radiation from the base station on Wednesday
        radtxsig = radiator(txsig[:,m],txang) 

        # Signal propagation from the base station to the target
        txchansig = txchannel(radtxsig,tx_pos,tgt_pos,tx_vel,tgt_vel)

        # Reflection of the signal from the target
        tgtsig = target(txchansig)

        # Signal propagation from the target to the I-th radar
        rxchansig = rxchannel(
            tgtsig,radar_pos[:,rad_i],tgt_pos, 
            radar_vel[:,rad_i],tgt_vel
        )

        # Calculating the angle of sight of the radar target
        _,rxang = rangeangle(radar_pos[:,rad_i],tgt_pos)

        # Reception of the reflected signal of the I-th radar
        rxsig = collector(rxchansig,rxang)

        # pre-amplification of the received signal
        x[:,m] .= receiver(rxsig)
    end

    # recording of the received I-th radar signal
    X[:,:,rad_i] .= x

    # Reset the position of the base station, radar and target
    reset_plt!(txplatform,txpos)
    reset_plt!(radarplatform,radarpos)
    reset_plt!(tgtplatform,tgtpos)
end

# Creating a TDOAEstimator object
tdoaEstimator = TDOAEstimator(
    SampleRateSource="Input port", # Sampling rate method (input port)
    VarianceOutputPort=true
)

# Calculation of estimates and variance of delays
Y2, tdoa_var_2 = tdoaEstimator(X, fs);
println("Delay estimates for each pair of radars: $(round.(Y2.*1e9;sigdigits=6)) ns")

plotTDOASpectrum(tdoaEstimator;
    AnchorPairIndex=1:4, # Radar pair numbers
    MaxDelay=1000e-9, # Maximum delay
    DinRange=60, # Dynamic range
    OneSidedSpectrum=false # building a one-way spectrum
)

tgtposest = tdoaposest(Y2,tdoa_var_2,radarpos)

println("Evaluation of the positions of $(round.(tgtposest;sigdigits=6)) m")
println("The true position of $(tgtpos) m")

helperPlotTDOAPositions(c,Y2,tgtposest,radarpos,tgtpos,txpos)

RMSE = rmse(tgtposest,tgtpos)
println("Positioning time: $(round(RMSE;sigdigits=6)) m")