import time
import serial
import enum


class Varid(enum.Enum):
	STATUS = 0,
	STATUS_ERRORS_OCCURED = 1,
	STATUS_SERIAL_ERRORS_OCCURED = 2,
	STATUS_LIMIT_STATUS = 3,
	STATUS_RESET_FLAGS = 127,
	RC_RC1_UNLIMITED_RAW_VALUE = 4,
	RC_RC1_RAW_VALUE = 5,
	RC_RC1_SCALED_VALUE = 6,
	RC_RC2_UNLIMITED_RAW_VALUE = 8,
	RC_RC2_RAW_VALUE = 9,
	RC_RC2_SCALED_VALUE = 10,
	ANALOG_AN1_UNLIMITED_RAW_VALUE = 12,
	ANALOG_AN1_RAW_VALUE = 13,
	ANALOG_AN1_SCALED_VALUE = 14,
	ANALOG_AN2_UNLIMITED_RAW_VALUE = 16,
	ANALOG_AN2_RAW_VALUE = 17,
	ANALOG_AN2_SCALED_VALUE = 18,
	DIAGNOSTIC_TARGET_SPEED = 20,
	DIAGNOSTIC_SPEED = 21,
	DIAGNOSTIC_BRAKE_AMOUNT = 22,
	DIAGNOSTIC_INPUT_VOLTAGE = 23,
	DIAGNOSTIC_TEMPERATURE_A = 24,
	DIAGNOSTIC_TEMPERATURE_B = 25,
	DIAGNOSTIC_RC_PERIOD = 26,
	DIAGNOSTIC_BAUDRATE_REGISTER = 27,
	DIAGNOSTIC_UP_TIME_LOW = 28,
	DIAGNOSTIC_UP_TIME_HIGH = 29,
	MOTOR_MAX_SPEED_FORWARD = 30,
	MOTOR_MAX_ACCEL_FORWARD = 31,
	MOTOR_MAX_DECEL_FORWARD = 32,
	MOTOR_BREAK_DURATION_FORWARD = 33,
	MOTOR_STARTING_SPEED_FORWARD = 34,
	MOTOR_MAX_SPEED_REVERSE = 36,
	MOTOR_MAX_ACCEL_REVERSE = 37,
	MOTOR_MAX_DECEL_REVERSE = 38,
	MOTOR_BREAK_DURATION_REVERSE = 39,
	MOTOR_STARTING_SPEED_REVERSE = 40,
	MOTOR_CURRENT_LIMIT = 41,
	MOTOR_CURRENT_RAW = 42,
	MOTOR_CURRENT_MA = 43,
	MOTOR_CURRENT_LIMIT_CONSECUTIVE_COUNT = 44,
	MOTOR_CURRENT_LIMIT_OCCURRENCE_COUNT = 45


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()

		self.stop()

		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 stop(self):
		return self.cmd("X")

	def motor_forward(self, speed_percent):
		return self.cmd("F" + str(int(speed_percent)) + "%")

	def motor_reverse(self, speed_percent):
		return self.cmd("R" + str(int(speed_percent)) + "%")

	def motor_brake(self, brake_amount_percent):
		return self.cmd("B" + str(int(brake_amount_percent)) + "%")

	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 int(self.cmd("D" + str(var.value[0])))

	def print_vars(self):
		print ("-------------------------------------------")
		print ("STATUS                   : {:04X}".format(self.get_variable(Varid.STATUS)))
		print ("STATUS_ERROR_OCCURED     : {:04X}".format(self.get_variable(Varid.STATUS_SERIAL_ERRORS_OCCURED)))
		print ("STATUS_LIMIT_STATUS      : {:04X}".format(self.get_variable(Varid.STATUS_LIMIT_STATUS)))
		print ("RC_RC1_SCALED_VALUE      : {:04X}".format(self.get_variable(Varid.RC_RC1_SCALED_VALUE)))
		print ("RC_RC2_SCALED_VALUE      : {:04X}".format(self.get_variable(Varid.RC_RC2_SCALED_VALUE)))
		print ("ANALOG_AN1_SCALED_VALUE  : {:04X}".format(self.get_variable(Varid.ANALOG_AN1_SCALED_VALUE)))
		print ("ANALOG_AN2_SCALED_VALUE  : {:04X}".format(self.get_variable(Varid.ANALOG_AN2_SCALED_VALUE)))
		print ("DIAGNOSTIC_TARGET_SPEED  : {:04X}".format(self.get_variable(Varid.DIAGNOSTIC_TARGET_SPEED)))
		print ("DIAGNOSTIC_SPEED         : {:04X}".format(self.get_variable(Varid.DIAGNOSTIC_SPEED)))
		print ("DIAGNOSTIC_TEMPERATURE_A : {:04X}".format(self.get_variable(Varid.DIAGNOSTIC_TEMPERATURE_A)))
		print ("DIAGNOSTIC_TEMPERATURE_B : {:04X}".format(self.get_variable(Varid.DIAGNOSTIC_TEMPERATURE_B)))
#		print ("MOTOR_CURRENT_RAW        : {:04X}".format(self.get_variable(Varid.MOTOR_CURRENT_RAW)))
#		print ("MOTOR_CURRENT_MA         : {:04X}".format(self.get_variable(Varid.MOTOR_CURRENT_MA)))


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

	drv = Pololu1376(serial_port=ser)
	drv.go()
	time.sleep(1)
	
	for speed in range(0, 70, 5):
		print("Speed = {}".format(speed))
		drv.motor_forward(speed)
		for n in range (0, 10, 1):
			drv.print_vars()
			time.sleep(0.5)
		drv.motor_brake(10)
		time.sleep(1)
		
	print("End of program")