import serial
from datetime import datetime
import socket

# Lookup table for cage axis to magnetometer transformation
# First entry: Magnetometer y axis to cage x (reversed)
# axis_mapping = [[1, -1 ...]
axis_mapping = [[2, -1], [0, 1], [1, -1]]

# Helmholtz control software tcp port
s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
s.connect(("localhost", 6677))
s.sendall("declare_api_version 2\n".encode())

# FGM3D software virtual serial port
ser = serial.Serial("COM11")

line = b""
ready = False
while True:
    line += ser.read()
    if line[-2:] == b"\r\n":
        new_line = line[:-2].decode('ascii')
        delta = datetime.now().timestamp()*1000 - int(new_line.split(';')[0])+(2*60*60*1000)
        if delta < 500 and not ready:
            ready = True
            print("Program ready!")
        if ready:
            # Data is not valid otherwise
            # Only use the x y and z values
            tokens = [float(v.replace(',', '.')) for v in new_line.split(";")[1:-1]]
            axis_fields = [tokens[m[0]]*m[1] for m in axis_mapping]
            axis_fields = ["{:.6e}".format(v) for v in axis_fields]
            s.sendall(("magnetometer_field {} {} {}\n".format(*axis_fields)).encode())
        line = b""