import serial
import time

from parser_mmw_demo import parser_one_mmw_demo_output_packet

# 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):
    
    # Raspberry pi
    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)

# 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():
    sensor_data = bytearray()
    bytes_parsed = 0
    while True:
        read_data = Dataport.read(1024)
        print(f"Read: {read_data.hex()}")
        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)

        # 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


if __name__ == "__main__":
    try:
        parse_frames()
    except KeyboardInterrupt:
        CLIport.write("sensorStop\n".encode())
        exit(0)