Pkg.add(["LinearAlgebra", "DSP"])

using DSP,FFTW,LinearAlgebra

function run_model( name_model, path_to_folder ) # defining a function for running the model
    Path = path_to_folder * "/" * name_model * ".engee"
    if name_model in [m.name for m in engee.get_all_models()] # Checking the condition for loading a model into the kernel
        model = engee.open( name_model ) # Open the model
        engee.run( model, verbose=true ); # Launch the model
        engee.close( name_model, force=true ); # Close the model
    else
        model = engee.load( Path, force=true ) # Upload a model
        engee.run( model, verbose=true ); # Launch the model
        engee.close( name_model, force=true ); # Close the model
    end
    return
end;

function DataFrame2Array(X)
    out = collect(X)
    out_data = zeros(eltype(out.value[1]),size(out.value[1],1),size(out.value[1],2),length(out.value))
    [out_data[:,:,i] = out.value[i] for i in 1:length(out.value)]

    return out_data, out.time
end

function calc_spectrogram(x::Array,fs::Real,title::String;
    num_pulses::Int64 = 4,
    window_length::Int64 = 64,
    nfft::Int64=512,
    lap::Int64  = 60,
    thesh::Real = -Inf)

    gr()

    spec_out = DSP.spectrogram(x[:,:,1:num_pulses][:],window_length,lap;window=kaiser(window_length,3.95),nfft=nfft,fs=fs)

    power = DSP.pow2db.(spec_out.power)
    power .= map(x -> x < thesh ? thesh : x,power)

    power_new = zeros(size(power))

    power_new[1:round(Int64,size(power,1)/2),:] .= power[round(Int64,size(power,1)/2)+1:end,:]
    power_new[round(Int64,size(power,1)/2)+1:end,:] .= power[1:round(Int64,size(power,1)/2),:]

    fig = heatmap(fftshift(spec_out.freq).*1e-6,spec_out.time .*1e6, permutedims(power_new),color= :jet,
        gridalpha=0.5,margin=5Plots.mm,size=(900,400))
    xlabel!("Frequency, MHz")
    ylabel!("Time, iss")
    title!(title)

    return fig
end

function calc_RD_Response(x,param;tresh=-Inf)
    plotlyjs()
    range_scale = Vector(range(param.RngLims...,length=size(x,1)))
    speed_scale = Vector(range(param.SpeedLims...,length=size(x,2)))

    out_RD = pow2db.(abs.(x[:,:,end]))
    out_RD  .= rot180(map(x -> x < tresh ? tresh : x,out_RD))

    fig = heatmap(speed_scale,range_scale,
        out_RD,colorscale=:parula256,
        top_margin=10Plots.mm,size=(900,400))
    xlabel!("Speed, m/s")
    ylabel!("Range, m")
    title!("Range-Doppler portrait")
    return fig
end

function plotting_result(rng_est,speed_est,true_range,true_speed,name::String,slow_time)
    plotlyjs()
    fig1=plot(slow_time*1e3,rng_est,
        lab="distance estimation",
        margin=5Plots.mm,
        legend=:outertopright,
        titlefont=font(12,"Computer Modern"),
        guidefont = font(12,"Computer Modern"),size=(900,220))
    plot!(fast_time*1e3,true_range,lab="The true distance",ls= :dash)
    xlabel!("Time, ms")
    ylabel!("Distance, m")
    title!("Dependence of the distance to the object ($(name))")

    fig2 = plot(slow_time*1e3,speed_est,
        lab="speed assessment",
        size=(900,220),
        legend=:outertopright,
        titlefont=font(12,"Computer Modern"),
        guidefont = font(12,"Computer Modern"))
    plot!(fast_time*1e3,true_speed,lab="true speed",ls= :dash)
    xlabel!("Time, ms")
    ylabel!("Speed, m/s")
    title!("Dependence of the relative velocity of an object ($name)")

    display(fig1)
    display(fig2)
    return
end

calc_range_or_speed_object(x,y) = [norm(x[:,1,i]).-norm(y[:,1,i]) for i in 1:size(y,3)];

include("$(@__DIR__)/calcParamFMCWMT.jl") # connecting a jl file to initialize input parameters
paramRadarFMCWMT = calcParamFMCWMT() # creating a structure of input parameters
T = paramRadarFMCWMT.T; # the sampling step of the model in time
SimT = 10*paramRadarFMCWMT.NumSweeps*paramRadarFMCWMT.T; # simulation time of the model

run_model("FMCW_Radar_Range_Estimation_MT",@__DIR__) # Launch the model

FMCW_signal, fast_time = DataFrame2Array(FMCW_sig) # the LCHM input signal
Range_Estimate, _ = DataFrame2Array(Range) # range estimation
Speed_Estimate, slow_time = DataFrame2Array(Speed) # speed estimates
RD_Response,_  = DataFrame2Array(RD); # range-Doppler portrait
Truck_vel,_ = DataFrame2Array(Tvel) # truck speed
Truck_pos,_ = DataFrame2Array(Tpos); # truck position
Car_vel,_ = DataFrame2Array(Cvel) # vehicle speed
Car_pos,_ = DataFrame2Array(Cpos); # vehicle position
Radar_vel,_ = DataFrame2Array(Rvel) # radar speed
Radar_pos,_ = DataFrame2Array(Rpos); # radar position

calc_spectrogram(
    FMCW_signal, # signal after processing (displayed with 2 pulses)
    paramRadarFMCWMT.Fs/paramRadarFMCWMT.NumSweeps, # sampling rate
    "The spectrogram of the input signal";
    num_pulses=4, # number of pulses
    thesh=-300 # threshold, dB
)

calc_RD_Response(RD_Response,paramRadarFMCWMT;tresh=-Inf)

# Calculation of true ranges and relative speeds to the vehicle
true_range_car = calc_range_or_speed_object(Car_pos,Radar_pos)
true_speed_car = calc_range_or_speed_object(Radar_vel,Car_vel)
# Calculation of true ranges and relative speeds to the truck
true_range_truck = calc_range_or_speed_object(Truck_pos,Radar_pos)
true_speed_truck = calc_range_or_speed_object(Radar_vel,Truck_vel)
# Calculation of range estimates and relative speeds to the vehicle
Range_Estimate_Car = Range_Estimate[1,1,:]
Range_Estimate_Truck = Range_Estimate[2,1,:]
# Calculation of estimated ranges and relative speeds to the truck
Speed_Estimate_Car = Speed_Estimate[1,1,:]
Speed_Estimate_Truck = Speed_Estimate[2,1,:];

 # graphs for comparing an object - a car
 plotting_result(Range_Estimate_Car,Speed_Estimate_Car,true_range_car,true_speed_car,"car",slow_time)

 # graphs for comparing a cargo object
plotting_result(Range_Estimate_Truck,Speed_Estimate_Truck,true_range_truck,true_speed_truck,"truck",slow_time)