import serial 
from time import sleep 


class meArm:
    position = [90,90,90,90] #home position
    moves = [] #stored moves
    port = "" #serial port
    baudrate = 9600
    ser = serial.Serial()

    def __init__(self, port = '/dev/ttyACM0', bautrate = 9600):
        self.port = port 
        self.baudrate = bautrate
        self.ser = serial.Serial(self.port, self.baudrate, timeout = 1)

    def addMove(self, moves = [b'D1\n']):
        self.moves.append(moves)
        return len(self.moves) -1

    def getMoves(self):
        [print(i, x) for i, x in enumerate(self.moves)]

    #default cmd H1 sends current pos over serial
    def sendCmd(self, cmd = b'H1'):
        self.ser.flushInput() #Last change. if broken remove this
        while(1):
            line = self.ser.readline()
            # print(f"sendCmd: {line}")
            if(line == b'READY\r\n'):
                break
        self.ser.write(cmd)
        self.ser.write(b'\n')
        # print(f"sendCmd: wrote: {cmd}")

    def execMove(self, idx):
        if(idx >= len(self.moves)):
            print("invalid movement index")
            return
        for cmd in self.moves[idx]:
            print(f"sending: {cmd}")
            self.sendCmd(cmd)
        return len(self.moves[idx])


    def getPos(self):
        self.sendCmd()
        sleep(0.1)
        self.ser.flushInput()
        c = 0
        while(1):
            line = self.ser.readline()
            print(line)
            if(line == b'READY\r\n'):
                c+=1
                if c ==2: break






















if __name__ == '__main__':
    m1 =  [b'T70', b'L40', b'R150', b'G180', b'R90', b'D1']
    m2 =  [b'S1', b'T70', b'L40', b'R150', b'G180', b'R90', b'T140', b'L50', b'R150', b'G90', b'D1']
    m3 =  [b'S1', b'T140', b'L50', b'R150', b'G180', b'D1']
    arm = meArm()
    arm.getPos()
    i = arm.addMove(m1)
    i = arm.addMove(m2)
    i = arm.addMove(m3)

    arm.execMove(1)
    arm.getPos()
    arm.execMove(2)