#include "nbus_slave.h"


#if MODULE_SLAVE == 1

void nbus_slave_unicastToSensorGet(nBus_TypeDef *nbus){
	switch(nbus->function_code.function){
	case CMD_SENSOR_TYPE:
	{
		nBus_sensorType_t t = nbus->interface->getType(nbus->sensorInfo.address);
		if (t == TYPE_UNKNOWN) {
			setErrorResponse(nbus, ILLEGAL_DEVICE_ADDRESS);
			break;
		}
		nbus->tx_buffer[4] = t;
		nbus->tx_length += 1;
	}
	break;

	case CMD_PARAM:
	{
		if (nbus->rx_length >= 5){
			if (nbus->interface->hasParam(nbus->sensorInfo.address, (nBus_param_t)nbus->rx_buffer[3]) == 0) {
				setErrorResponse(nbus, PARAM_NOT_IMPLEMENTED);
				break;
			}
			nbus->tx_buffer[4] = nbus->rx_buffer[3];
			int32_t param_value = nbus->interface->getParam(nbus->sensorInfo.address, (nBus_param_t)nbus->rx_buffer[3]);
			nbus->tx_buffer[5] = (uint8_t)(param_value & 0xFF);
			nbus->tx_buffer[6] = (uint8_t)((param_value>>8) & 0xFF);
			nbus->tx_buffer[7] = (uint8_t)((param_value>>16) & 0xFF);
			nbus->tx_buffer[8] = (uint8_t)((param_value>>24) & 0xFF);
			nbus->tx_length += 5;
		} else {
			nBus_param_t* params = nbus_interface_allParams();
			for(uint8_t i=0; i < nbus_interface_allParamsCount() ; i++){
				if(nbus->interface->hasParam(nbus->sensorInfo.address, params[i])){
					int32_t param_value = nbus->interface->getParam(nbus->sensorInfo.address, params[i]);
					nbus->tx_buffer[4+5*i] = params[i];
					//nbus->tx_buffer[5+2*i] = nbus->interface->getParam(nbus->sensorInfo.address, params[i]);
					nbus->tx_buffer[5+5*i] = (uint8_t)(param_value & 0xFF);
					nbus->tx_buffer[6+5*i] = (uint8_t)((param_value>>8) & 0xFF);
					nbus->tx_buffer[7+5*i] = (uint8_t)((param_value>>16) & 0xFF);
					nbus->tx_buffer[8+5*i] = (uint8_t)((param_value>>24) & 0xFF);

					nbus->tx_length += 5;
				}
			}

		}
	}
	break;

	case CMD_DATA:
	{
		if (nbus->measure_active == MEASURE_RUNNING) {
			uint8_t cnt = nbus->interface->getData(nbus->sensorInfo.address, &nbus->tx_buffer[4]);
			if (cnt == 0){
				setErrorResponse(nbus, DEVICE_BUSY);
				//return;
			}
			nbus->tx_length += cnt;
		} else {
			setErrorResponse(nbus, DEVICE_NOT_READY);
		}

	}
	break;

	default:
	{
		setErrorResponse(nbus, ILLEGAL_FUNCTION);
	}

	}
}

void nbus_slave_unicastToSensorSet(nBus_TypeDef *nbus){
	switch(nbus->function_code.function){
	case CMD_PARAM:
	{
		if (!nbus->interface->hasParam(nbus->sensorInfo.address, (nBus_param_t)nbus->rx_buffer[3])){
			setErrorResponse(nbus, PARAM_NOT_IMPLEMENTED);
			break;
		}

		int32_t param_value = nbus->rx_buffer[4] | nbus->rx_buffer[5]<<8 | nbus->rx_buffer[6]<<16 << nbus->rx_buffer[7]<<23;
		nBus_param_t p = nbus->interface->setParam(nbus->sensorInfo.address, (nBus_param_t)nbus->rx_buffer[3], param_value);
		if (p == PARAM_NONE) {
			setErrorResponse(nbus, ILLEGAL_DATA_VALUE);
			break;
		}
		nbus->tx_buffer[4] = OK_CODE;
		nbus->tx_length += 1;
	}
	break;

	case CMD_DATA:
	{
		nbus->tx_buffer[4] = nbus->interface->setData(&nbus->rx_buffer[3]);
		if (nbus->tx_buffer[4] != OK_CODE){
			nbus->function_code.error = 1;
		}
		nbus->tx_length += 1;
	}
	break;

	default:
	{
		setErrorResponse(nbus, ILLEGAL_FUNCTION);
	}
	}
}

#endif