#include "P60DockComIF.h"

#include <csp/csp.h>
#include <csp/drivers/can_socketcan.h>
#include <bsp_linux/comIF/cookies/P60DockCookie.h>
#include <fsfw/serviceinterface/ServiceInterfaceStream.h>
#include <gomspace/libparam_client/include/gs/param/rparam.h>

P60DockComIF::P60DockComIF(object_id_t objectId) :
		SystemObject(objectId) {

}

P60DockComIF::~P60DockComIF() {
}

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

	P60DockCookie* p60DockCookie = dynamic_cast<P60DockCookie*>(cookie);
	uint8_t cspAddress = p60DockCookie->getCspAddress();
	char* canInterface = p60DockCookie->getCanIf();
	int bitrate = p60DockCookie->getBitrate();

	int buf_count = 10;
	int buf_size = 300;
	/* Init CSP and CSP buffer system */
	if (csp_init(cspAddress) != 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 = 512;
	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;
	}
	return HasReturnvaluesIF::RETURN_OK;
}

ReturnValue_t P60DockComIF::sendMessage(CookieIF *cookie,
		const uint8_t * sendData, size_t sendLen) {
	P60DockCookie* p60DockCookie = dynamic_cast<P60DockCookie*> (cookie);
	MessageType_t messageType = p60DockCookie->getMessageType();

	switch(messageType){
	case(P60DockCookie::REBOOT):{
		csp_reboot(p60DockCookie->getCspAddress());
		break;
	}
	default:
		break;
	}
	return HasReturnvaluesIF::RETURN_OK;
}

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

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

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

	MessageType_t messageType = p60DockCookie->getMessageType();

	switch(messageType){
	case(P60DockCookie::READ_MODULE_CONFIG):{
		uint32_t timeout = 1000;
		uint8_t p60dockAddress = p60DockCookie->getCspAddress();
		gs_param_table_instance_t moduleConfig;
		moduleConfig.rows = (gs_param_table_row_t*)p60dock_config;
		moduleConfig.id = moduleCfgTableNum;
		moduleConfig.row_count = p60dock_config_count;
		moduleConfig.memory_size = p60dock_config_size;
		moduleConfig.memory = replyBuffer;
		/* Read complete module configuration table from P60 Dock and store data
		 * in buffer */
		int result = gs_rparam_get_full_table(&moduleConfig, p60dockAddress,
				moduleConfig.id, GS_RPARAM_MAGIC_CHECKSUM, timeout);
		*size = moduleCfgTableSize;
		if (result != GS_OK) {
			sif::info
					<< "Failed retrieving module configuration from P60 dock "
					<< "with error code " << result << std::endl;
			return HasReturnvaluesIF::RETURN_FAILED;
		}
		break;
	}
	case(P60DockCookie::READ_HK):{
		uint32_t timeout = 1000;
		uint8_t p60dockAddress = p60DockCookie->getCspAddress();
		tmData.rows = (gs_param_table_row_t*)p60dock_hk;
		tmData.id = tmTableNum;
		tmData.row_count = p60dock_hk_count;
		tmData.memory_size = P60DOCK_HK_SIZE;
		tmData.memory = replyBuffer;
		/* Read complete module configuration table from P60 Dock and store data
		 * in buffer */
		int result = gs_rparam_get_full_table(&tmData, p60dockAddress,
				tmData.id, GS_RPARAM_MAGIC_CHECKSUM, timeout);
		*size = tmTableSize;
		if (result != GS_OK) {
			sif::info
					<< "Failed retrieving telemetry from P60 dock with error "
				    << "code " << result << std::endl;
			return HasReturnvaluesIF::RETURN_FAILED;
		}
		break;
	}
	default:
		break;
	}
	return HasReturnvaluesIF::RETURN_OK;
}