import struct
import time

import serial

import mspdef


class Link:
    def __init__(self, port):
        self.port = port
        self.buf = None

    def request(self, msg):
        self.send(msg.command())

    def push(self, msg):
        self.send(msg.command(), mspdef.encode(msg))

    def send(self, command, data=None):
        data = data or b''
        chk = len(data) ^ command
        for ch in data:
            chk ^= ch
        self.write(b'$M<' + struct.pack('<BB', len(data), command) +
                   bytes(data) + bytes((chk, )))

    def write(self, b):
        self.port.write(b)

    def get(self) -> int:
        if not self.buf:
            w = self.port.in_waiting or 1
            self.buf = self.port.read(w)
            print('< %r' % self.buf)
        if not self.buf:
            raise Exception('Read timeout')
        ch, self.buf = self.buf[0], self.buf[1:]
        return ch

    def read(self):
        while True:
            if self.get() != ord('$'):
                print('sync')
                continue
            if self.get() != ord('M'):
                print('not-m')
                continue
            if self.get() != ord('>'):
                print('not->')
                continue
            size = self.get()
            command = self.get()
            payload = []
            for _ in range(size):
                payload.append(self.get())
            chk = command ^ size
            for p in payload:
                chk ^= p
            if self.get() != chk:
                print('chk')
                continue
            return command, payload


def main():
    p = serial.Serial('/dev/ttyUSB0', 115200, timeout=1)
    l = Link(p)
    aux1 = 1000
    ch1 = 1000
    #    print('\033[;H\033[J')
    while True:
        ch1 += 999
        if ch1 >= 2000:
            ch1 = 1000
        #        print('\033[;H')
        for t in mspdef.Status, :
            l.request(t())
            command, payload = l.read()
            s = mspdef.decode(t(), payload)
            print(s)
            if s.arming_disable_flags == 0:
                aux1 = 1500
        l.push(mspdef.SetRawRC([ch1, ch1, 1000, ch1, aux1]))
        command, payload = l.read()
        print(mspdef.decode(mspdef.SetRawRC(), payload))
        time.sleep(0.1)


main()