import pickle

from datetime import datetime

import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d.axes3d import Axes3D

# Turning interactive mode on for plotting
plt.ion()
fig = plt.figure()

ax = fig.add_subplot(projection="3d")

# making the plot axis constant
ax.set_xlim(0, 1)
ax.set_ylim(0, 1)
ax.set_zlim(0, 1)

plt.show()


def plot_data(timestamp: datetime,
              parser_result,
              header_start_index,
              total_packet_size,
              detected_objects,
              tlv_count,
              sub_frame_number,
              detected_points):
    
    detected_x, detected_y, detected_z, detected_v, detected_range, detected_azimuth, detected_elevation, detected_snr, detected_noise = detected_points

    ax.clear()

    # Setting axis labels
    ax.set_xlabel("X axis (m)")
    ax.set_ylabel("Y axis (m)")
    ax.set_zlabel("Z axis (m)")

    ax.set_title(timestamp.strftime("%Y-%m-%d %H:%M:%S"))

    ax.set_autoscale_on(False)
    ax.scatter(detected_x, detected_y, detected_z)
    plt.pause(0.2)


def main():
    with open('2025-06-11_recording.bin', mode='rb') as file:
        while True:
            try:
                data = pickle.load(file)
                plot_data(**data)
            except EOFError:
                break


if __name__ == "__main__":
    try:
        main()
    except KeyboardInterrupt:
        exit(0)