import serial
import time

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

from parser_mmw_demo import parser_one_mmw_demo_output_packet

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

# Change the configuration file name
configFileName = 'hedgehog.cfg'

# ------------------------------------------------------------------

# Function to configure the serial ports and send the data from
# the configuration file to the radar
def serialConfig(configFileName):
    
    # Serial device ports
    CLIport = serial.Serial('/dev/ttyACM0', 115200)
    Dataport = serial.Serial('/dev/ttyACM1', 921600)

    # Read the configuration file and send it to the board
    with open(configFileName, mode="rt", encoding="utf-8") as config_file:
        for line in config_file:
            line = line.rstrip("\r\n")
            print(f"[CONFIG] writing: '{line}'")

            CLIport.write((line + "\n").encode())
            time.sleep(0.01)

    return CLIport, Dataport

# -------------------------    MAIN   -----------------------------------------  

# Configurate the serial port
CLIport, Dataport = serialConfig(configFileName) # Enable when reading from sensor
# Dataport = open("sensor_data.bin", mode="rb") # Enable when reading from file

# parser_one_mmw_demo_output_packet extracts only one complete frame at a time
# so call this in a loop till end of file

def parse_frames():

    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()

    sensor_data = bytearray()
    bytes_parsed = 0
    while True:
        read_data = Dataport.read(256)
        with open("sensor_data.bin", mode="ab") as fp:
            fp.write(read_data)

        sensor_data += bytearray(read_data)
        
        # parser_one_mmw_demo_output_packet function already prints the
        # parsed data to stdio. So showcasing only saving the data to arrays 
        # here for further custom processing
        parser_result, \
        headerStartIndex,  \
        totalPacketNumBytes, \
        numDetObj,  \
        numTlv,  \
        subFrameNumber,  \
        detectedX_array,  \
        detectedY_array,  \
        detectedZ_array,  \
        detectedV_array,  \
        detectedRange_array,  \
        detectedAzimuth_array,  \
        detectedElevation_array,  \
        detectedSNR_array,  \
        detectedNoise_array = parser_one_mmw_demo_output_packet(sensor_data[bytes_parsed::1], len(sensor_data) - bytes_parsed)

        ax.clear()

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

        ax.set_autoscale_on(False)
        ax.scatter(detectedX_array, detectedY_array, detectedZ_array)
        plt.pause(0.01)

        # Check the parser result
        print ("Parser result: ", parser_result)
        if (parser_result == 0): 
            bytes_parsed += (headerStartIndex+totalPacketNumBytes)
            print("totalBytesParsed: ", bytes_parsed)
        else:
            # bytes_parsed += 1
            continue

if __name__ == "__main__":
    try:
        parse_frames()
    except KeyboardInterrupt:
        CLIport.write("sensorStop\n".encode()) # Enable when reading from sensor
        # Dataport.close() # Enable when reading from file
        exit(0)