/*
 * P60DockTestTask.cpp
 *
 *  Created on: 18.11.2020
 *      Author: Jakob Meier
 */

#include <fsfw/serviceinterface/ServiceInterfaceStream.h>

#include <csp/drivers/can_socketcan.h>
#include "P60DockTestTask.h"

P60DockTestTask::P60DockTestTask(object_id_t objectId_):
SystemObject(objectId_){
	if(initializeCSPStack() != HasReturnvaluesIF::RETURN_OK){
		sif::error << "P60DockTestTask creation failed" << std::endl;
	}
}


ReturnValue_t P60DockTestTask::performOperation(uint8_t operationCode) {

	if(sendPacket() != HasReturnvaluesIF::RETURN_OK){
		return HasReturnvaluesIF::RETURN_FAILED;
	}
	return HasReturnvaluesIF::RETURN_OK;
}


ReturnValue_t P60DockTestTask::sendPacket(void){

//	char *msg = "HELLO";
//	/* Get packet buffer for data */
//	csp_packet_t *packet = csp_buffer_get(strlen(msg));
//	if (packet == NULL) {
//		/* Could not get buffer element */
//		sif::error("Failed to get buffer element\\n");
//		return HasReturnvaluesIF::RETURN_FAILED;
//	}
//
//	/* Connect P60 Dock */
//	csp_conn_t *conn = csp_connect(CSP_PRIO_NORM, c, CSP_PING,
//			1000, CSP_O_NONE);
//
//	if (conn == NULL) {
//		/* Connect failed */
//		sif::error("Connection failed\\n");
//		/* Remember to free packet buffer */
//		csp_buffer_free(packet);
//	return HasReturnvaluesIF::RETURN_FAILED;
//	}
//
//	/* Copy message to packet */
//	strcpy(packet->data, msg);
//	/* Set packet length */
//	packet->length = strlen(msg);
//
//	/* Send packet */
//	if (!csp_send(conn, packet, 1000)) {
//		/* Send failed */
//		sif::error("Send failed\\n");
//		csp_buffer_free(packet);
//	}
//	/* Close connection */
//	csp_close(conn);

	uint32_t timeout = 1000;
	unsigned int pingSize = 100; // 100 bytes
	uint32_t replyTime = csp_ping(p60dockAddress, timeout, pingSize, CSP_O_NONE);
	sif::info << "Ping address: " << p60dockAddress << ", reply after "
			<< replyTime << "ms" << std::endl;
	return HasReturnvaluesIF::RETURN_OK;
}


ReturnValue_t P60DockTestTask::initializeCSPStack(void){
	/* Init CSP and CSP buffer system */
	if (csp_init(cspAddress) != CSP_ERR_NONE
			|| csp_buffer_init(10, 300) != CSP_ERR_NONE) {
		sif::error << "Failed to init CSP\r\n" << std::endl;
		return HasReturnvaluesIF::RETURN_FAILED;
	}

	csp_iface_t *csp_if_ptr = &csp_if;
	csp_if_ptr = csp_can_socketcan_init("can0", bitrate, promisc);

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


P60DockTestTask::~P60DockTestTask() {
	// TODO Auto-generated destructor stub
}