import senti_pb2 as senti
import sentipy

def print_header(h):
    print(f"port={h.source} counter={h.counter} toa={h.toa_ns} tov={h.tov_ns} utc_s={h.utc_s} utc_ns={h.utc_ns}")

def senti_print_message(m):
    print(m)

# SB Generated Messages
def handle_trigger(m):
    print_header(m.header)
def handle_input_capture(m):
    print(m)
def handle_raw(m):
    print_header(m.header)
    # Print sensor package as hex
    print(m.payload.hex())

# GNSS Generated Messages
def handle_gnss_pos_llh(m):
    print(m)
def handle_gnss_pos_ecef(m):
    print(m)
def handle_gnss_relpos_ned(m):
    if(m.fix == senti.RTK_FIX):
        print(m)
def handle_gnss_rfi_status(m):
    print(m)
def handle_gnss_status(m):
    print(m)
def handle_gnss_time(m):
    print(m)
def handle_gnss_vel_ecef(m):
    print(m)
def handle_gnss_vel_ned(m):
    print(m)
def handle_gnss_dop(m):
    print(m)

# IMU Generated Messages
def handle_imu(m):
    print(f'lin_acc=[{m.lin_acc[0]:7.2f}, {m.lin_acc[1]:7.2f}, {m.lin_acc[2]:7.2f}] ang_vel=[{m.ang_vel[0]:7.2f}, {m.ang_vel[1]:7.2f}, {m.ang_vel[2]:7.2f}] temp={m.temperature:.1f}')
def handle_imu_mag(m):
    print(m)
def handle_imu_mag_orientation(m):
    print(m)

UDP_IP = ""
UDP_PORT = 21314

parser = sentipy.SentiMessageParser(UDP_IP, UDP_PORT)

parser.register_callback(senti.RAW_MSG, handle_raw)
parser.register_callback(senti.GNSS_RELPOS_NED, handle_gnss_relpos_ned)
parser.register_callback(senti.IMU_MAG_ORIENTATION_MSG, handle_imu)

parser.connect()

while True:
    parser.receive_message()