/*
 * 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(pingP60dock() != HasReturnvaluesIF::RETURN_OK){
		return HasReturnvaluesIF::RETURN_FAILED;
	}

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


ReturnValue_t P60DockTestTask::pingP60dock(void){
	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) {
	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 = hkMem;
	/* Retriev all houskeeping data from the P60 dock and store it in hkMem
	 * array */
	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" << std::endl;
		return HasReturnvaluesIF::RETURN_FAILED;
	} else {
		uint8_t tableOffsetTemperature = 0x44;
		int16_t temperature;
		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: " << temperature << std::endl;

		uint16_t vbat_v;
		parameterSize = sizeof(vbat_v);
		uint8_t vbat_v_offset = 0x74;
		result = gs_param_get_data((gs_param_table_instance_t*) &node_hk,
				vbat_v_offset, &vbat_v, parameterSize, flags);
		sif::info << "VBAT_V: " << vbat_v << std::endl;
	}
	return HasReturnvaluesIF::RETURN_OK;
}


ReturnValue_t P60DockTestTask::initializeCSPStack(void){
	/* Init CSP and CSP buffer system */
	if (csp_init(cspClientAddress) != 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
}