#Antzer calling SDK with Python example
#=========import==========
import struct
import threading
import time
from ctypes import *
from ctypes import cdll, c_uint, POINTER
from enum import IntEnum
from datetime import datetime
import numpy as np
#Load library file
dev0_clib = cdll.LoadLibrary('./libfaro_can_sdk.so')
#Devices Influence each other whom call the same library in an application
#Copy and rename the library file.
dev1_clib = cdll.LoadLibrary('./libfaro_can_sdk_2.so')
dev_clib = [dev0_clib, dev1_clib]
#=========Define==========
MAIN_VER = 1
MINOR_VER = 0
ANTZER_DEV_MAX = 1

DWREAD_Type = c_uint32
CAN_BAUD_RATE_TYPE = c_int

#CAN Baud rate
CAN_SPEED_1M   = CAN_BAUD_RATE_TYPE(0)
CAN_SPEED_800K = CAN_BAUD_RATE_TYPE(1)
CAN_SPEED_500K = CAN_BAUD_RATE_TYPE(2)
CAN_SPEED_250K = CAN_BAUD_RATE_TYPE(3)
CAN_SPEED_200K = CAN_BAUD_RATE_TYPE(4)
CAN_SPEED_125K = CAN_BAUD_RATE_TYPE(5)

DEV0_NAME = bytes("/dev/antzerCAN0","gbk") # Device name
DEV1_NAME = bytes("/dev/antzerCAN1","gbk") # Device name
DEV0_UART_BR = 5                        # 1.5M bps
DEV1_UART_BR = 5                        # 1.5M bps
DEV0_CAN_PORT0_BR = CAN_SPEED_1M      # Set port0 baud rate
DEV0_CAN_PORT1_BR = CAN_SPEED_1M      # Set port1 baud rate
DEV1_CAN_PORT0_BR = CAN_SPEED_1M      # Set port0 baud rate
DEV1_CAN_PORT1_BR = CAN_SPEED_1M      # Set port1 baud rate

DEV_CAN_PORT0_BR = [DEV0_CAN_PORT0_BR, DEV1_CAN_PORT0_BR]
DEV_CAN_PORT1_BR = [DEV0_CAN_PORT1_BR, DEV1_CAN_PORT1_BR]

#=========Structure==========
class can_fw_ver_t(Structure):
    _fields_ = [("main",c_ubyte),
                ("minor",c_ubyte),
                ("type",c_ubyte),
                ("year",c_ubyte),
                ("week",c_ubyte),
                ("sub1",c_ubyte),
		("sub2",c_ubyte),
                ("rc",c_ubyte)]

# port = 0 or 1
# speed = CAN_SPEED_1M to CAN_SPEED_125K
class can_config_t(Structure):
	_fields_ = [("port",c_ubyte),
		("speed",c_int)]

class can_message_t(Structure):
	_fields_ = [("port",c_ubyte),
		("id",c_uint32),
                ("mode",c_ubyte), #CAN 2.0A/B (STD/Extended)
                ("rtr",c_ubyte),  #Remote request
                ("dlc",c_ubyte),
                ("data",c_ubyte*8)]

#=========Function==========			
def can_start():
	can_fw_ver  = can_fw_ver_t()
	for i in range(ANTZER_DEV_MAX):
		retVer = dev_clib[i].AZ_VC_GetFWVer(byref(can_fw_ver)) #Show FARO/GAND FW version
		if retVer != 0:
			print("Dev:%d, Calling AZ_VC_GetFWVer fail"%(i))
		else:
			print("Dev:" + hex(i) + " , V" + hex(can_fw_ver.main) + "," + hex(can_fw_ver.minor) + "," + hex(can_fw_ver.type) + "," + hex(can_fw_ver.year) + "," + hex(can_fw_ver.week))

		retPort0Mode = dev_clib[i].AZ_VC_ModeActive(0x00,0x00)	#set port0 mode to RAW CAN mode
		if retPort0Mode != 0:
			print("ERROR: Dev:%d, Failed to switch port0 mode"%(i))
		else:
			print("Dev:%d, Succeed to switch port0 mode"%(i))

		retPort1Mode = dev_clib[i].AZ_VC_ModeActive(0x01,0x00)	#set port1 mode to RAW CAN mode
		if retPort0Mode != 0:
			print("ERROR: Dev:%d, Failed to switch port1 mode"%(i))
		else:
			print("Dev:%d, Succeed to switch port1 mode"%(i))

		can_cfg = can_config_t()
		can_cfg.port = 0  #Port 0
		can_cfg.speed = DEV_CAN_PORT0_BR[i] #500k bps
		retCanCfg = dev_clib[i].AZ_VC_CAN_Config(can_cfg) #Set port0 baud rate
		if retCanCfg != 0:
			print("ERROR: Dev:%d, Set baud rate of CAN port0 fail"%(i))
		else:
			print("Dev:%d, Set baud rate of CAN port0 succeed"%(i))

		can_cfg.port = 1  #Port1
		can_cfg.speed = DEV_CAN_PORT1_BR[i] #500k bps
		retCanCfg = dev_clib[i].AZ_VC_CAN_Config(can_cfg) #Set port1 baud rate
		if retCanCfg != 0:
			print("ERROR: Dev:%d, Set baud rate of CAN port1 fail"%(i))
		else:
			print("Dev:%d, Set baud rate of CAN port1 succeed"%(i))

def can_disable():
	can_cfg = can_config_t()
	for i in range(ANTZER_DEV_MAX):	
		can_cfg.port = 0  #Port 0
		can_cfg.speed = 8 #OFF
		retCanCfg = dev_clib[i].AZ_VC_CAN_Config(can_cfg) #Set port0 baud rate
		if retCanCfg != 0:
			print("ERROR: Dev:%d, CAN P0 disable fail"%(i))
			#exit(0)

		can_cfg.port = 1  #Port1
		can_cfg.speed = 8 #OFF

		retCanCfg = dev_clib[i].AZ_VC_CAN_Config(can_cfg) #Set port1 baud rate
		if retCanCfg != 0:
			print("ERROR: Dev:%d, CAN P1 disable fail"%(i))

def send_thread():  #Send CAN data thread
	try:
		dev0_p0_send_can_data = can_message_t()
		dev0_p0_send_can_data.port = 0
		dev0_p0_send_can_data.id = 0x7E8
		dev0_p0_send_can_data.mode = 0
		dev0_p0_send_can_data.rtr = 0
		dev0_p0_send_can_data.dlc = 8
		dev0_p0_send_can_data.data[0] = 0x03
		dev0_p0_send_can_data.data[1] = 0x41
		dev0_p0_send_can_data.data[2] = 0x0D
		dev0_p0_send_can_data.data[3] = 0x06
		dev0_p0_send_can_data.data[4] = 0xAA
		dev0_p0_send_can_data.data[5] = 0xAA
		dev0_p0_send_can_data.data[6] = 0xAA
		dev0_p0_send_can_data.data[7] = 0xAA

		dev1_p0_send_can_data = can_message_t()
		dev1_p0_send_can_data.port = 0
		dev1_p0_send_can_data.id = 0xCF00400
		dev1_p0_send_can_data.mode = 1
		dev1_p0_send_can_data.rtr = 0
		dev1_p0_send_can_data.dlc = 8
		dev1_p0_send_can_data.data[0] = 0xFF
		dev1_p0_send_can_data.data[1] = 0xFF
		dev1_p0_send_can_data.data[2] = 0x7D
		dev1_p0_send_can_data.data[3] = 0x80
		dev1_p0_send_can_data.data[4] = 0x6D
		dev1_p0_send_can_data.data[5] = 0xEF
		dev1_p0_send_can_data.data[6] = 0xFF
		dev1_p0_send_can_data.data[7] = 0xFF
		
		for i in range(30):
			ret = dev0_clib.AZ_VC_CAN_Write(dev0_p0_send_can_data)
			if ret != 0:
				break

			ret = dev1_clib.AZ_VC_CAN_Write(dev1_p0_send_can_data)
			if ret != 0:
				break
			time.sleep(1) #sleep 1s
		print("Send End")
	finally:
		print("Send thread End")

#=========Main function==========
if __name__=="__main__":
	try:
		print("V%d,%d"%(MAIN_VER, MINOR_VER)) #Show python code version

		ret = dev0_clib.AZ_VC_Init_PB(DEV0_NAME,DEV0_UART_BR, 100) #1.5M bps
		if ret !=0:
			print("Dev:0, Opendevice fail!")
			exit(0)
		else:
        		print("Dev:0, Opendevice succeed!")
		if ANTZER_DEV_MAX > 1:
			ret = dev1_clib.AZ_VC_Init_PB(DEV1_NAME,DEV1_UART_BR, 100) #1.5M bps
			if ret !=0:
				print("Dev:1, Opendevice fail!")
				exit(0)
			else:
        			print("Dev:1, Opendevice succeed!")
		
		can_start() #Init CAN
		#receive initial
		#rec_can_data = can_message_t()
		rec_can_data = (can_message_t * 20000)()
		#rec_can_data = np.zeros(20000, can_message_t())
		dwread = DWREAD_Type(0)
		dataLeng = 0
		p0_rec_count = [0,0]
		p1_rec_count = [0,0]
		cnt_temp = 0
		#send initial
		# Create thread
		sendT = threading.Thread(target = send_thread)
		# Run thread
#		sendT.start() #Enable it for sending data.
		print('Start sending/receiving data')
		while True:
			time.sleep(0.01) #10 ms #You could modify it to get data immediately. For instance: time.sleep(0.001)
			for i in range(ANTZER_DEV_MAX):
				dataLeng = dev_clib[i].AZ_VC_CAN_Read_Amount()
				retRecCan = dev_clib[i].AZ_VC_CAN_Read_Array(byref(rec_can_data), dataLeng)
				if retRecCan > 0:
					for j in range(retRecCan):
						if rec_can_data[j].port == 0:
							p0_rec_count[i]+=1
							cnt_temp = p0_rec_count[i]
						else:
							p1_rec_count[i]+=1
							cnt_temp = p1_rec_count[i]
						if (cnt_temp%1000) == 0:
							print("Dev:%d, GetNum:%d ,port:%x ,Timestamp:%s"%(i, cnt_temp,rec_can_data.port,str(datetime.utcnow())))
	
	finally:	
		can_disable()
		print(p0_rec_count)
		print(p1_rec_count)
		time.sleep(2)
		ret=dev0_clib.AZ_VC_DeInit()
		if ret !=0:
			print("Closedevice0 Failed!")
		else:
			print("Closedevice0 succeed!")
		del dev0_clib
		if ANTZER_DEV_MAX > 1:
			ret=dev1_clib.AZ_VC_DeInit()
			if ret !=0:
				print("Closedevice1 Failed!")
			else:
				print("Closedevice1 succeed!")
		
			del dev1_clib
		print('done')
