#include "FitCANBus.h"
#include <queue>
#include <thread>
#include <future>
#include <mutex>
#include <vector>
using namespace std;

DWORD64 start_cycle;

#define F81601_ONLY 0
#define CAN_IER_SETTING 0

class CanRecord{
public:
	FITAPI::CFitekCanBusControl::CanFrameInfor msg;
	int count;
	CanRecord(FITAPI::CFitekCanBusControl::CanFrameInfor* message);
};

CanRecord::CanRecord(FITAPI::CFitekCanBusControl::CanFrameInfor* msg) {
	memcpy(&this->msg, msg, sizeof(FITAPI::CFitekCanBusControl::CanFrameInfor));
	count = 1;
}

vector<CanRecord*> TxRecords;
vector<CanRecord*> RxRecords;

void CheckRecords(vector<CanRecord*> &records, FITAPI::CFitekCanBusControl::CanFrameInfor* message) {
	for (size_t i = 0; i < records.size(); i++) {
		if (memcmp(records[i], message, sizeof(FITAPI::CFitekCanBusControl::CanFrameInfor)) == 0) {
			records[i]->count++;
			return;
		}
	}
	records.push_back(new CanRecord(message));
}

void appendMessage(char* str, size_t size, FITAPI::CFitekCanBusControl::CanFrameInfor* msg) {
	if (msg->type == FITAPI::CFitekCanBusControl::CanFrameFormat::CP_11Bit)
		_snprintf_s(str, size, _TRUNCATE, "%s%d(%03Xh)[%d]", str, msg->id, msg->id, msg->data_len);
	else
		_snprintf_s(str, size, _TRUNCATE, "%s%d(%08Xh)[%d]", str, msg->id, msg->id, msg->data_len);
	for (int i = 0; i < msg->data_len; i++) {
		_snprintf_s(str, size, _TRUNCATE, "%s %02X", str, msg->data[i]);
	}
	printf_s("\n");
}

FitCANBus::FitCANBus(char* sComPortNumber) {
	printf_s("FitCANBus CAN bus = %s\n", sComPortNumber);
	strcpy_s(asComPortNumber, sComPortNumber);
	hCan = new FITAPI::CFitekCanBusControl();
}

FitCANBus::~FitCANBus() {
	long ret;
	unsigned long lasterror = 0;

	ret = hCan->FintekCanbus_GetErrorCode(asComPortNumber, &lasterror);
	if (ret < 0) {
		printf_s("Unable to get error code ret= %x\n", ret);
	}

	ret = hCan->FintekCanbus_Close(asComPortNumber);
	printf_s("Uninitialize CAN bus = %x error = %x\n", ret, lasterror);
	Sleep(100);

	// delete records
	for (size_t i = 0; i < RxRecords.size(); i++)
		delete RxRecords[i];
	RxRecords.clear();
	for (size_t i = 0; i < TxRecords.size(); i++)
		delete TxRecords[i];
	TxRecords.clear();
}

long FitCANBus::init(int baudrate) {
	long ret;
	FILE* fd = NULL;	
	start_cycle = __rdtsc();
	printf_s("Start\n");

	printf_s("FintekCanbus_Open\n");	

	
	// Initial CAN bus. (It is matter)
	ret = hCan->FintekCanbus_Open(asComPortNumber);
	if (ret < 0) {
		printf_s("Failed to initialise CAN bus IO controller %x\n", ret);
		return ret;
	}

#if F81601_ONLY
	UCHAR F81601Id;
	ret = hCan->FintekCanbus_GetIdVal(asComPortNumber, &F81601Id);
	if (ret < 0) {
		printf_s("Unable to get F81601 Id %x\n", ret);
		return ret;
	}
	printf_s("F81601 id: %d\n", F81601Id);
#endif	

	// Set CAN bus baudrate. (It is matter)
	ret = hCan->FintekCanbus_SetBaudRate(asComPortNumber, baudrate);
	if (ret < 0) {
		printf_s("Unable to set CAN bus baud rate %x\n", ret);
		return ret;
	}
	// Confirm the CAN bus baudRate.
	DWORD baudRate;
	ret = hCan->FintekCanbus_GetBaudRate(asComPortNumber, &baudRate);
	if (ret < 0) {
		printf_s("Unable to get CAN bus baud rate %x\n", ret);
		return ret;
	}
	printf_s("CAN Bus Baud Rate: %d\n", baudRate);

#if CAN_IER_SETTING
	unsigned char ucIer = 0xEF;
	// Set CAN bus IER.
	ret = hCan->FintekCanbus_SetIer(asComPortNumber, ucIer);
	if (ret < 0) {
		printf_s("Unable to set CAN bus IER %x\n", ret);
		return ret;
	}
	// Confirm the CAN bus IER.	
	ret = hCan->FintekCanbus_GetIer(asComPortNumber, &ucIer);
	if (ret < 0) {
		printf_s("Unable to get CAN bus IER %x\n", ret);
		return ret;
	}
	printf_s("CAN Bus IER: 0x%X\n", ucIer);
#endif

	return ret;
}

static DWORD64 rxCnt = 0;
static DWORD64 errorCnt1 = 0;
static DWORD64 errorCnt2 = 0;

void CALLBACK callback(long error, FITAPI::CFitekCanBusControl::CanFrameInfor* message)
{
	int i;
	rxCnt++;
	if (error) {
		printf_s("Read error: %Xh\n", error);
		if ((error == 0x80050FFF) || (error == 0x8005FFFF)) {
			errorCnt2++;
		}
		else {
			errorCnt1++;
		}
	}
	else {
		char tmp[MAX_PATH];
		CheckRecords(RxRecords, message);
		if (message->rtr == 1)
		{
			_snprintf_s(tmp, sizeof(tmp), "[RX]RTR[%d]", message->rtr);
		}
		else
		{
			if (message->type == FITAPI::CFitekCanBusControl::CanFrameFormat::CP_11Bit)
				_snprintf_s(tmp, sizeof(tmp), "[RX]%03Xh[%d]", message->id, message->data_len);
			else
				_snprintf_s(tmp, sizeof(tmp), "[RX]%08Xh[%d]", message->id, message->data_len);

			for (i = 0; i < message->data_len; i++) {
				_snprintf_s(tmp, sizeof(tmp), "%s %02X", tmp, message->data[i]);
			}
		}
		printf_s(tmp);
		printf_s("\n");
	}
}

long FitCANBus::startReading(int num, DWORD* patterns, DWORD* masks)
{
	long ret;
	// set CAN read software filter
	for (int i = 0; i < num; i++) {
		ret = hCan->FintekCanbus_SetFilter(asComPortNumber, patterns[i], masks[i]);
		if (ret < 0) {
			printf_s("Unable to set CAN filter %x\n", ret);
		}
	}

	// set CAN filter (acr/amr)
	ret = hCan->FintekCanbus_SetAcrAmrFilter(asComPortNumber, CANBUS_SINGLE_FILTER_MODE, 0, 0xFFFFFFFF);
	if (ret < 0) {
		printf_s("Unable to set CAN filter (acr/arm) %x\n", ret);
	}

	// set CAN read callback
	ret = hCan->FintekCanbus_Receive(asComPortNumber, callback);
	if (ret < 0) {
		printf_s("Unable to set CAN message read callback %x\n", ret);
	}
	return ret;
}

long FitCANBus::FintekCanbus_SetId(DWORD id, FITAPI::CFitekCanBusControl::CanFrameFormat type) {
	long ret;
	printf_s("Set CANID %d(%Xh)\n", id, id);
	ret = hCan->FintekCanbus_SetId(asComPortNumber, type, id);
	if (ret < 0) {
		printf_s("Unable to Set CANID %d, %Xh ret= %x\n", id, id, ret);
	}
	return ret;
}

long FitCANBus::writeMessage(FITAPI::CFitekCanBusControl::CanFrameInfor* msg)
{
	long ret;
#ifdef DEBUG_MSG
	char tmp[MAX_PATH];
	CheckRecords(TxRecords, msg);
	strcpy_s(tmp, "[TX]");
	appendMessage(tmp, sizeof(tmp), msg);
	printf(tmp);
#endif
	ret = hCan->FintekCanbus_Send(asComPortNumber, msg);
#ifdef DEBUG_MSG
	if (msg->type == FITAPI::CFitekCanBusControl::CanFrameFormat::CP_11Bit)
		printf_s("[TX]%03Xh[%d] done\n", msg->id, msg->data_len);
	else
		printf_s("[TX]%08Xh[%d] done\n", msg->id, msg->data_len);
#endif
	if (ret < 0) {
		printf_s("Unable to write %Xh message %x\n", msg->id, msg->data_len);
	}
	return ret;
}