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

#include <fsfw/serviceinterface/ServiceInterfaceStream.h>
#include "P60DockTestTask.h"

#include <csp/drivers/can_socketcan.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;
	}

	if(getParameters() != 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::getParameters(void) {
//	int result = rparam_get_full_table(&node_hk, p60dock_node, P60_PORT_RPARAM,
	uint32_t timeout = 1000;
	node_hk.rows = (gs_param_table_row_t*)p60dock_hk;
	node_hk.id = P60DOCK_HK;
	node_hk.row_count = p60dock_hk_count;
	node_hk.memory_size = P60DOCK_HK_SIZE;
	node_hk.memory = hk_mem;
	int result = gs_rparam_get_full_table(&node_hk, p60dockAddress, node_hk.id,
			GS_RPARAM_MAGIC_CHECKSUM, timeout);

	if (result != 0) {
		sif::info << "Error retrieving P60 Dock housekeeping\n" << std::endl;
		return HasReturnvaluesIF::RETURN_FAILED;
	} else {
		uint8_t tableOffsetTemperature = 0x44;
		int16_t temperature[2];
		size_t parameterSize = sizeof(temperature);
		uint32_t flags = 0;
		result = gs_param_get_data((gs_param_table_instance_t*) &node_hk,
				tableOffsetTemperature, temperature, parameterSize, flags);
		sif::info << "P60 Dock Temperature 1: " << temperature[0] << std::endl;
		sif::info << "P60 Dock Temperature 2: " << temperature[1] << std::endl;

//		sif::info << "Retrieved P60 Dock housekeeping\n" << std::endl;
//		/* List all out_en[] values, using parameter name */
//		const param_table_t * param = param_find_name(node_hk.table,
//				node_hk.count, "out_en");
//		if (param != NULL) {
//			for (uint8_t index = 0; index < 13; index++) {
//				/* Read parameter using name */
//				uint8_t *out_en = param_read_addr(
//						param->addr + param->size * index, &node_hk,
//						param->size);
//				sif::info << "out_en" << index << ": " << *out_en << std::endl;
//			}
//		}
//		/* List all c_out[] values, using parameter address */
//		param = param_find_addr(node_hk.table, node_hk.count, 0x0000);
//		if (param != NULL) {
//			for (uint8_t index = 0; index < 13; index++) {
//				/* Read parameter using address */
//				int16_t *c_out = param_read_addr(
//						param->addr + param->size * index, &node_hk,
//						param->size);
//				sif::info << "c_out" << index << ": " << *c_out << "mA"
//						<< 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
}