#include "CspComIF.h"
#include <bsp_linux/comIF/cookies/CspCookie.h>
#include <fsfw/serviceinterface/ServiceInterfaceStream.h>
#include <csp/drivers/can_socketcan.h>
#include <fsfw/serialize/SerializeAdapter.h>

CspComIF::CspComIF(object_id_t objectId) :
		SystemObject(objectId) {

}

CspComIF::~CspComIF() {
}

ReturnValue_t CspComIF::initializeInterface(CookieIF *cookie)  {
	if(cookie == nullptr) {
		return NULLPOINTER;
	}

	CspCookie* cspCookie = dynamic_cast<CspCookie*>(cookie);
	if(cspCookie == nullptr) {
		return NULLPOINTER;
	}
	char* canInterface = cspCookie->getCanIf();
	int bitrate = cspCookie->getBitrate();
	/* Define the memory to allocate for the CSP stack */
	int buf_count = 10;
	int buf_size = 300;
	/* Init CSP and CSP buffer system */
	if (csp_init(cspClientAddress) != CSP_ERR_NONE
			|| csp_buffer_init(buf_count, buf_size) != CSP_ERR_NONE) {
		sif::error << "Failed to init CSP\r\n" << std::endl;
		return HasReturnvaluesIF::RETURN_FAILED;
	}

	int promisc = 0; // Set filter mode on
	csp_iface_t *csp_if_ptr = &csp_if;
	csp_if_ptr = csp_can_socketcan_init(canInterface, bitrate, promisc);

	/* Set default route and start router */
	uint8_t address = CSP_DEFAULT_ROUTE;
	uint8_t netmask = 0;
	uint8_t mac = CSP_NODE_MAC;
	int result = csp_rtable_set(address, netmask, csp_if_ptr, mac);
	if(result != CSP_ERR_NONE){
		sif::error << "Failed to add can interface to router table"
				<< std::endl;
		return HasReturnvaluesIF::RETURN_FAILED;
	}

	/* Start the route task */
	unsigned int task_stack_size = 500;
	unsigned int priority = 0;
	result = csp_route_start_task(task_stack_size, priority);
	if(result != CSP_ERR_NONE){
		sif::error << "Failed to start csp route task" << std::endl;
		return HasReturnvaluesIF::RETURN_FAILED;
	}

	uint8_t cspAddress = cspCookie->getCspAddress();
	uint16_t maxReplyLength = cspCookie->getMaxReplyLength();
	if(cspDeviceMap.find(cspAddress) == cspDeviceMap.end()){
		/* Insert device information in CSP map */
		cspDeviceMap.emplace(cspAddress, vectorBuffer(maxReplyLength));
	}

	return HasReturnvaluesIF::RETURN_OK;
}

ReturnValue_t CspComIF::sendMessage(CookieIF *cookie,
		const uint8_t * sendData, size_t sendLen) {
	int result;
	if(cookie == NULL){
		return HasReturnvaluesIF::RETURN_FAILED;
	}
	CspCookie* cspCookie = dynamic_cast<CspCookie*> (cookie);
	if(cspCookie == NULL){
		return HasReturnvaluesIF::RETURN_FAILED;
	}

	/* Extract csp port and bytes to query from command buffer */
	uint8_t cspPort;
	uint16_t querySize;
	SerializeAdapter::deSerialize(&cspPort, &sendData, &sendLen,
			SerializeIF::Endianness::BIG);
	SerializeAdapter::deSerialize(&querySize, &sendData, &sendLen,
			SerializeIF::Endianness::BIG);
	uint8_t cspAddress = cspCookie->getCspAddress();
	if(cspPort == csp_reserved_ports_e::CSP_PING){
		uint32_t timeout = 1000; // ms
		unsigned int pingSize = 100; // 100 bytes
		uint32_t replyTime = csp_ping(cspAddress, timeout, pingSize,
				CSP_O_NONE);
		sif::info << "Ping address: " << cspAddress << ", reply after "
				<< replyTime << " ms" << std::endl;
		/* Store reply time in reply buffer * */
		uint8_t* replyBuffer = cspDeviceMap[cspAddress].data();
		memcpy(replyBuffer, &replyTime, sizeof(replyTime));
	}
	else if(cspPort == csp_reserved_ports_e::CSP_REBOOT){
		csp_reboot(cspCookie->getCspAddress());
	}
	else{
		/* No CSP fixed port was selected. Send data to the specified port and
		 * wait for querySize number of bytes */
		result = cspTransfer(cspAddress, cspPort, sendData, sendLen,
				querySize);
		if(result != HasReturnvaluesIF::RETURN_OK){
			return HasReturnvaluesIF::RETURN_FAILED;
		}
		rememberQuerySize = querySize;
	}
	return HasReturnvaluesIF::RETURN_OK;
}

ReturnValue_t CspComIF::getSendSuccess(CookieIF *cookie) {
	return HasReturnvaluesIF::RETURN_OK;
}

ReturnValue_t CspComIF::requestReceiveMessage(CookieIF *cookie,
		size_t requestLen) {
	return HasReturnvaluesIF::RETURN_OK;
}

ReturnValue_t CspComIF::readReceivedMessage(CookieIF *cookie,
		uint8_t** buffer, size_t* size) {
	if(cookie == NULL){
		return HasReturnvaluesIF::RETURN_FAILED;
	}
	CspCookie* cspCookie = dynamic_cast<CspCookie*> (cookie);
	if(cspCookie == NULL){
		return HasReturnvaluesIF::RETURN_FAILED;
	}

	uint8_t cspAddress = cspCookie->getCspAddress();

	*buffer = cspDeviceMap[cspAddress].data();
	*size = rememberQuerySize;

	return HasReturnvaluesIF::RETURN_OK;
}

ReturnValue_t CspComIF::cspTransfer(uint8_t cspAddress, uint8_t cspPort,
		const uint8_t* cmdBuffer, int cmdBufferLen, uint16_t querySize) {

	uint32_t timeout_ms = 1000;
	vectorBufferIter iter = cspDeviceMap.find(cspAddress);
	if(iter == cspDeviceMap.end()){
		sif::error << "CSP device with address " << cspAddress << " no found in"
				<< " device map" << std::endl;
	}
	uint8_t* replyBuffer = iter->second.data();
	uint8_t tmpCmdBuffer[cmdBufferLen];
	memcpy(tmpCmdBuffer, cmdBuffer, cmdBufferLen);

	csp_conn_t * conn = csp_connect(CSP_PRIO_HIGH, cspAddress, cspPort, 0,
			CSP_O_NONE);

	int result = csp_transaction_persistent(conn, timeout_ms,
			tmpCmdBuffer, cmdBufferLen, replyBuffer, querySize);
	if(querySize != 0){
		if(result != querySize){
			sif::error << "CSP transfer failed to receive all requested bytes "
					   << std::endl;
			return HasReturnvaluesIF::RETURN_FAILED;
		}
	} else {
		if(result != 1){
			sif::error << "CSP transfer failed" << std::endl;
			return HasReturnvaluesIF::RETURN_FAILED;
		}
	}

	csp_close(conn);

	return HasReturnvaluesIF::RETURN_OK;
}