import socket, struct, time, numpy, math

beacon = {
    "ip": "127.0.0.1",
    "port": 49011
    }
sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)

datarefs = [
    ("sim/flightmodel/position/latitude"),
    ("sim/flightmodel/position/longitude"),
    ("sim/flightmodel/position/elevation"),
    ("sim/cockpit2/gauges/indicators/heading_AHARS_deg_mag_copilot"),
    ("sim/cockpit2/gauges/indicators/pitch_AHARS_deg_pilot"),
    ("sim/cockpit2/gauges/indicators/roll_AHARS_deg_pilot")
]

# Send read request
state = []
for idx, dref in enumerate(datarefs):
    cmd = b"RREF\x00"
    freq = 1
    msg = struct.pack("<4sxii400s", cmd, freq, idx, dref.encode())
    sock.sendto(msg, (beacon["ip"], beacon["port"]))

# Record reading data
a = time.time()
b = time.time()
while (b-a) < 0.5:
    a = time.time()
    data, addr = sock.recvfrom(1024) # buffer size is 1024 bytes
    imp_data = data[5:]
    idx = 0
    value = 0
    for i in range(6):
        mydata = imp_data[(8*i):(8*(i+1))]
        (idx, val) = struct.unpack("<if", mydata)
        state.append(val)
    b = time.time()

# Stop read request
for idx, dref in enumerate(datarefs):
    cmd = b"RREF\x00"
    freq = 0
    msg = struct.pack("<4sxii400s", cmd, freq, idx, dref.encode())
    sock.sendto(msg, (beacon["ip"], beacon["port"]))


# Write data
print( "Angle requested: ", state[3] )

msg = struct.pack('<4sxidddfff', b'VEHS',
                  0,              # The index of the airplane you want to control.
                  state[0],        # longitude, in degrees
                  state[1],        # latitude, in degrees
                  state[2],        # elevation above sea level, in meters
                  state[3],        # heading, degrees true
                  state[4],       # pitch, degrees
                  state[5])      # roll, degrees

sock.sendto(msg, (beacon['ip'], beacon['port']))