#!/usr/bin/env python3

import sys
import struct
import serial

# Useage
if len(sys.argv) != 2:
    print("Usage: {} /dev/ttyACMx".format(sys.argv[0]))
    sys.exit(1)

# Message Type Definitions
MESSAGE_PVT         = 1
MESSAGE_PSU         = 2         
MESSAGE_RANGING     = 4     
MESSAGE_POSITION    = 8
MESSAGE_RX_PACKET   = 51   

# TOAD ID Definitions
TOAD_1 = 1
TOAD_2 = 2
TOAD_3 = 4
TOAD_4 = 8
TOAD_5 = 16
TOAD_6 = 32

# Packet Types
RANGE_PACKET = 64
POSITION_PACKET = 128

# Get TOAD ID from range/position packet
def get_toad_id_from_type(packet_type):

    if((packet_type & TOAD_1) == TOAD_1):
        print("Generated by TOAD 1")    
    
    if((packet_type & TOAD_2) == TOAD_2):
        print("Generated by TOAD 2") 
        
    if((packet_type & TOAD_3) == TOAD_3):
        print("Generated by TOAD 3") 
        
    if((packet_type & TOAD_4) == TOAD_4):
        print("Generated by TOAD 4") 
        
    if((packet_type & TOAD_5) == TOAD_5):
        print("Generated by TOAD 5") 
    
    if((packet_type & TOAD_6) == TOAD_6):
        print("Generated by TOAD 6")
        
        
# Open Serial Port
ser = serial.Serial(sys.argv[1])
print("Listening on", ser.name)

# Fetch & Decode
while True:

    # Read in a Log
    data = ser.read(128)
      
    # Get Message Metadata
    meta_data = struct.unpack('<BBI', data[0:6])
    log_type = meta_data[0]
    toad_id = meta_data[1]
    systick = meta_data[2]
    systick /= 10000.0
    
    # Handle PVT Message
    if (log_type == MESSAGE_PVT):
                   
        payload = data[6:98]
        pvt = struct.unpack('<IHBBBBBBIiBBBBiiiiIIiiiiiIIHHIiI', payload)
        print("PVT MESSAGE:")
        print("TOAD ID = ", toad_id)
        print("Timestamp = ", systick, " s")
        print("Log Type = ", log_type)
        print("i_tow = ", pvt[0])
        print("year = ", pvt[1])
        print("month = ", pvt[2])
        print("day = ", pvt[3])
        print("hour = ", pvt[4])
        print("minute = ", pvt[5])
        print("second = ", pvt[6])
        print("valid = ", pvt[7])
        print("t_acc = ", pvt[8])
        print("nano = ", pvt[9])
        print("fix_type = ", pvt[10])
        print("flags = ", pvt[11])
        print("num_sv = ", pvt[13])
        print("lon = ", (pvt[14]/10000000), "degrees")
        print("lat = ", (pvt[15]/10000000), "degrees")
        print("height = ", (pvt[16]/1000), "m")
        print("h_msl = ", (pvt[17]/1000), "m")
        print("h_acc = ", pvt[18])
        print("v_acc = ", pvt[19])
        print("velN = ", pvt[20])
        print("velE = ", pvt[21])
        print("velD = ", pvt[22])
        print("gspeed = ", pvt[23])
        print("head_mot = ", pvt[24])
        print("s_acc = ", pvt[25])
        print("head_acc = ", pvt[26])
        print("p_dop = ", pvt[27])
        print("head_veh = ", pvt[30])
        print('\n\n')
    
    # Handle PSU Message
    if (log_type == MESSAGE_PSU):
        payload = data[6:13]
        psu = struct.unpack('<HHBBB', payload)
        print("PSU MESSAGE:")
        print("TOAD ID = ", toad_id)
        print("Timestamp = ", systick, " s")
        print("Log Type = ", log_type)
        print("battery voltage = ", (psu[1]/1000), "V")
        print("stm32 temp = ", psu[4], "degrees C")
        print("charging = ", psu[3])
        print("charge current = ", psu[0], "mA")
        print("charge temperature = ", psu[2], "degrees C")
        print('\n\n')
        
    # Handle Ranging Packet
    if (log_type == MESSAGE_RANGING):
        payload = data[6:17]
        ranging = struct.unpack('<BIIBB', payload)
        print("RANGING PACKET:")
        print("TOAD ID = ", toad_id)
        print("Timestamp = ", systick, " s")
        print("Log Type = ", log_type)
        get_toad_id_from_type(ranging[0])
        print("time of flight = ", ranging[1])
        print("i_tow = ", ranging[2])
        print ("battery voltage = ", (ranging[3]/10), "V")
        print("stm32 temp = ", ranging[4], "degrees C")
        print('\n\n')
        
    # Handle Position Packet
    if (log_type == MESSAGE_POSITION):
        payload = data[6:22]
        pos = struct.unpack('<BiiiBBB', payload)
        print("POSITION PACKET:")
        print("TOAD ID = ", toad_id)
        print("Timestamp = ", systick, " s")
        print("Log Type = ", log_type)
        get_toad_id_from_type(pos[0])
        print("lon = ", (pos[1]/10000000), "degrees")
        print("lat = ", (pos[2]/10000000), "degrees")
        print("height = ", (pos[3]/1000), "m")  
        print("num sat = ", pos[4])
        print("battery voltage = ", (pos[5]/10), "V")
        print("stm32 temp = ", pos[6], "degrees C")
        print('\n\n')
        
    # Handle SR Traffic - RX Packet Logged
    if (log_type == MESSAGE_RX_PACKET):
        print("SR TRAFFIC [RX Packet]:")
        print("TOAD ID = ", toad_id)
        print("Timestamp = ", systick, " s")            
        rx_type = data[6]
        get_toad_id_from_type(rx_type)
        
        # Handle Packet Types
        if ((rx_type & RANGE_PACKET) == RANGE_PACKET):
            payload = data[7:17]
            sr_rx_rp = struct.unpack('<IIBB', payload)
            print("time of flight = ", sr_rx_rp[0])
            print("i_tow = ", sr_rx_rp[1])
            print ("battery voltage = ", (sr_rx_rp[2]/10), "V")
            print("stm32 temp = ", sr_rx_rp[3], "degrees C")
            print('\n\n')
        
        if ((rx_type & POSITION_PACKET) == POSITION_PACKET):
            payload = data[7:22]
            sr_rx_pos = struct.unpack('<iiiBBB', payload)
            print("lon = ", (sr_rx_pos[0]/10000000), "degrees")
            print("lat = ", (sr_rx_pos[1]/10000000), "degrees")
            print("height = ", (sr_rx_pos[2]/1000), "m")  
            print("num sat = ", sr_rx_pos[3])
            print("battery voltage = ", (sr_rx_pos[4]/10), "V")
            print("stm32 temp = ", sr_rx_pos[5], "degrees C")
            print('\n\n')
