import time
import serial


class Pololu1376:
	def __init__(self, serial_port):
		self.ser = serial_port
		self.ser.set_output_flow_control(False)
		try:
			self.ser.open()
		except serial.SerialException:
			self.ser.close()
			self.ser.open()

		print("F/W-Version:", self.get_firmware_version())

	def send(self, data):
		self.ser.write((data + '\r\n').encode())

	def recv(self):
		data = self.ser.readline().decode("utf-8").replace("\n", '').replace("\r", '')
		return data

	def cmd(self, cmd):
		self.send(cmd)
		rsp = self.recv()

		if "." in rsp[0]:
			pass
		elif "!" in rsp[0]:
			pass
		elif "?" in rsp[0]:
			raise Exception("The last command was not understood (a Serial Format Error has occurred).")
		elif rsp is None:
			raise Exception("Timeout.")

		return rsp[1:]

	def go(self):
		return self.cmd("GO")

	def motor_forward(self):
		return self.cmd("F")

	def motor_reverse(self):
		return self.cmd("R")

	def motor_brake(self):
		return self.cmd("B")

	def motor_stop(self):
		return self.cmd("X")

	def motor_set_limit(self, value):
		self.cmd("L" + str(value))

	def get_firmware_version(self):
		return self.cmd("V")

	def get_variable(self, var):
		return self.cmd("L" + str(var))


if __name__ == '__main__':
	ser = serial.Serial("/dev/ttyACM0", 115200)

	s = Pololu1376(serial_port=ser)

	print("End of program")