#include "main.h"
#include <string.h>
#include <stdlib.h>
#include "MySensors.h"

#define	ICSC_SYS_PACK	0x58
#define SOH 1
#define STX 2
#define ETX 3
#define EOT 4

// message buffers
MyMessage _msg;			// Buffer for incoming messages
MyMessage _msgTmp;		// Buffer for temporary messages (acks and nonces among others)

// Receiving header information
char _header[6];

// Reception state machine control and storage variables
unsigned char _recPhase;
unsigned char _recPos;
unsigned char _recCommand;
unsigned char _recLen;
unsigned char _recStation;
unsigned char _recSender;
unsigned char _recCS;
unsigned char _recCalcCS;
unsigned char _packet_received;
UART_HandleTypeDef *_huart;

char _data[MY_RS485_MAX_MESSAGE_LENGTH];
uint8_t _packet_len;
unsigned char _packet_from;

_Bool send(MyMessage *message, uint8_t data_type)
{
	message->last = MY_NODE_ID;
	message->sender = MY_NODE_ID;
	message->destination = GATEWAY_ADDRESS;
	message->command_echo_payload = (data_type << 5) + C_SET;

	return transportSend(message);
}

_Bool sendHeartbeat(void)
{
	_msgTmp.last = MY_NODE_ID;
	_msgTmp.sender = MY_NODE_ID;
	_msgTmp.destination = GATEWAY_ADDRESS;
	_msgTmp.command_echo_payload = (P_ULONG32 << 5) + C_INTERNAL;
	_msgTmp.type = I_HEARTBEAT_RESPONSE;
	_msgTmp.version_length = (4 << 3) + V2_MYS_HEADER_PROTOCOL_VERSION;
	_msgTmp.ulValue = 0;

	return transportSend(&_msgTmp);
}

_Bool present(const uint8_t childSensorId, const mysensors_sensor_t sensorType, char *desc)
{
	_msgTmp.last = MY_NODE_ID;
	_msgTmp.sender = MY_NODE_ID;
	_msgTmp.destination = GATEWAY_ADDRESS;
	_msgTmp.command_echo_payload = (P_STRING << 5) + C_PRESENTATION;
	_msgTmp.type = sensorType;
	_msgTmp.sensor = childSensorId;
	_msgTmp.version_length = (strlen(desc) << 3) + V2_MYS_HEADER_PROTOCOL_VERSION;
	strcpy((char *)_msgTmp.data, desc);

	return transportSend(&_msgTmp);
}

void registerNode(void)
{
	_msgTmp.last = MY_NODE_ID;
	_msgTmp.sender = MY_NODE_ID;
	_msgTmp.destination = GATEWAY_ADDRESS;
	_msgTmp.command_echo_payload = (P_BYTE << 5) + C_INTERNAL;
	_msgTmp.type = I_REGISTRATION_REQUEST;
	_msgTmp.sensor = 0;
	_msgTmp.version_length = (1 << 3) + V2_MYS_HEADER_PROTOCOL_VERSION;
	_msgTmp.bValue = MY_CORE_VERSION;

	transportSend(&_msgTmp);
}

void sendLibraryInfo(void)
{
	_msgTmp.last = MY_NODE_ID;
	_msgTmp.sender = MY_NODE_ID;
	_msgTmp.destination = GATEWAY_ADDRESS;
	_msgTmp.command_echo_payload = (P_STRING << 5) + C_INTERNAL;
	_msgTmp.type = I_VERSION;
	_msgTmp.version_length = (strlen(MY_LIBRARY_VERSION) << 3) + V2_MYS_HEADER_PROTOCOL_VERSION;
	strcpy((char *)_msgTmp.data, MY_LIBRARY_VERSION);

	transportSend(&_msgTmp);
}

_Bool sendSketchInfo(const char *name, const char *version)
{
	_Bool result = 1;

	if (name) {
		_msgTmp.last = MY_NODE_ID;
	    _msgTmp.sender = MY_NODE_ID;
		_msgTmp.destination = GATEWAY_ADDRESS;
		_msgTmp.command_echo_payload = (P_STRING << 5) + C_INTERNAL;
		_msgTmp.type = I_SKETCH_NAME;
		_msgTmp.version_length = (strlen(name) << 3) + V2_MYS_HEADER_PROTOCOL_VERSION;
		strcpy((char *)_msgTmp.data, name);

		result &=  transportSend(&_msgTmp);
	}
	if (version) {
		_msgTmp.last = MY_NODE_ID;
	    _msgTmp.sender = MY_NODE_ID;
		_msgTmp.destination = GATEWAY_ADDRESS;
		_msgTmp.command_echo_payload = (P_STRING << 5) + C_INTERNAL;
		_msgTmp.type = I_SKETCH_VERSION;
		_msgTmp.version_length = (strlen(name) << 3) + V2_MYS_HEADER_PROTOCOL_VERSION;
		strcpy((char *)_msgTmp.data, version);

		result &=  transportSend(&_msgTmp);
	}

	sendLibraryInfo();

	return result;
}

// Message delivered through _msg
_Bool _processInternalCoreMessage(void)
{
	const uint8_t type = _msg.type;
	if (_msg.sender == GATEWAY_ADDRESS) {
		if (type == I_PRESENTATION) {
			// Re-send node presentation to controller
			present_node();
		} else if (type == I_HEARTBEAT_REQUEST) {
			(void)sendHeartbeat();
		} else if (type == I_REBOOT) {
			NVIC_SystemReset();
		} else if (type == I_VERSION) {
			sendLibraryInfo();
		} else {
			return 0; // further processing required
		}
	} else {
		return 0; // further processing required
	}
	return 1; // if not GW or no further processing required
}

void transportProcessMessage(void)
{
	// get message length and limit size
	const uint8_t msgLength = (_msg.version_length & 0xF8) >> 3;
	// calculate expected length

	const uint8_t command = _msg.command_echo_payload & 0x07;
	const uint8_t type = _msg.type;
	const uint8_t sender = _msg.sender;
	const uint8_t destination = _msg.destination;

	// Is message addressed to this node?
	if (destination == MY_NODE_ID) {
		// null terminate data
		_msg.data[msgLength] = 0u;
		// Check if sender requests an echo.
		if (_msg.command_echo_payload & 0x08) {
			memcpy(&_msgTmp, &_msg, sizeof(_msg));	// Copy message
			// Reply without echo flag (otherwise we would end up in an eternal loop)
			_msgTmp.command_echo_payload = _msgTmp.command_echo_payload & 0xE7;
			_msgTmp.command_echo_payload = _msgTmp.command_echo_payload | 0x10;
			_msgTmp.sender = MY_NODE_ID;
			_msgTmp.destination = sender;
			transportSend(&_msgTmp);
		}
		if(!(_msg.command_echo_payload & 0x10)) {
			// only process if not ECHO
			if (command == C_INTERNAL) {
				if (type == I_ID_RESPONSE) {
					return; // no further processing required
				}
				// general
				if (type == I_PING) {
					_msgTmp.last = MY_NODE_ID;
		        	_msgTmp.sender = MY_NODE_ID;
					_msgTmp.destination = sender;
					_msgTmp.command_echo_payload = (P_BYTE << 5) + C_INTERNAL;
					_msgTmp.type = I_PONG;
					_msgTmp.bValue = 1;
					transportSend(&_msgTmp);
					return; // no further processing required
				}
				if (type == I_PONG) {
					return; // no further processing required
				}
				if (_processInternalCoreMessage()) {
					return; // no further processing required
				}
			}
		}
		// Call incoming message callback if available
		receive(&_msg);
	} else if (destination == BROADCAST_ADDRESS) {
		if (command == C_INTERNAL) {
			if (type == I_DISCOVER_REQUEST) {

				HAL_Delay(MY_NODE_ID * 50);

				_msgTmp.last = MY_NODE_ID;
	        	_msgTmp.sender = MY_NODE_ID;
				_msgTmp.destination = sender;
				_msgTmp.command_echo_payload = (P_BYTE << 5) + C_INTERNAL;
				_msgTmp.type = I_DISCOVER_RESPONSE;
				_msgTmp.bValue = GATEWAY_ADDRESS;
				transportSend(&_msgTmp);
				return; // no further processing required
			}
		}
		if (command != C_INTERNAL) {
			receive(&_msg);
		}
	}
}

//Reset the state machine and release the data pointer
void _serialReset()
{
	_recPhase = 0;
	_recPos = 0;
	_recLen = 0;
	_recCommand = 0;
	_recCS = 0;
	_recCalcCS = 0;
}

// This is the main reception state machine.  Progress through the states
// is keyed on either special control characters, or counted number of bytes
// received.  If all the data is in the right format, and the calculated
// checksum matches the received checksum, AND the destination station is
// our station ID, then look for a registered command that matches the
// command code.  If all the above is true, execute the command's
// function.
_Bool _serialProcess()
{
	unsigned char i;
	if (!_byte_received) {
		return 0;
	}
	_byte_received = 0;

	switch(_recPhase) {

	// Case 0 looks for the header.  Bytes arrive in the serial interface and get
	// shifted through a header buffer.  When the start and end characters in
	// the buffer match the SOH/STX pair, and the destination station ID matches
	// our ID, save the header information and progress to the next state.
	case 0:
		memcpy(&_header[0],&_header[1],5);
		_header[5] = _byte;

		if ((_header[0] == SOH) && (_header[5] == STX) && (_header[1] != _header[2])) {
			_recCalcCS = 0;
			_recStation = _header[1];
			_recSender = _header[2];
			_recCommand = _header[3];
			_recLen = _header[4];

			for (i=1; i<=4; i++) {
				_recCalcCS += _header[i];
			}
			_recPhase = 1;
			_recPos = 0;

			//Avoid _data[] overflow
			if (_recLen >= MY_RS485_MAX_MESSAGE_LENGTH) {
				_serialReset();
				break;
			}

			//Check if we should process this message
			//We reject the message if we are the sender
			//We reject if we are not the receiver and message is not a broadcast
			if ((_recSender == MY_NODE_ID) ||
			        (_recStation != MY_NODE_ID &&
			         _recStation != BROADCAST_ADDRESS)) {
				_serialReset();
				break;
			}

			if (_recLen == 0) {
				_recPhase = 2;
			}

		}
		break;

	// Case 1 receives the data portion of the packet.  Read in "_recLen" number
	// of bytes and store them in the _data array.
	case 1:
		_data[_recPos++] = _byte;
		_recCalcCS += _byte;
		if (_recPos == _recLen) {
			_recPhase = 2;
		}
		break;

	// After the data comes a single ETX character.  Do we have it?  If not,
	// reset the state machine to default and start looking for a new header.
	case 2:
		// Packet properly terminated?
		if (_byte == ETX) {
			_recPhase = 3;
		} else {
			_serialReset();
		}
		break;

	// Next comes the checksum.  We have already calculated it from the incoming
	// data, so just store the incoming checksum byte for later.
	case 3:
		_recCS = _byte;
		_recPhase = 4;
		break;

	// The final state - check the last character is EOT and that the checksum matches.
	// If that test passes, then look for a valid command callback to execute.
	// Execute it if found.
	case 4:
		if (_byte == EOT) {
			if (_recCS == _recCalcCS) {
				// First, check for system level commands.  It is possible
				// to register your own callback as well for system level
				// commands which will be called after the system default
				// hook.

				switch (_recCommand) {
					case ICSC_SYS_PACK:
						_packet_from = _recSender;
						_packet_len = _recLen;
						_packet_received = 1;
						break;
				}
			}
		}
		//Clear the data
		_serialReset();
		//Return true, we have processed one command
		return 1;
		break;
	}
	return 1;
}

_Bool transportSend(MyMessage* data)
{
	const char *datap = (const char *)data;
	unsigned char i;
	unsigned char len;
	unsigned char cs = 0;

	// This is how many times to try and transmit before failing.
	unsigned char timeout = 10;

	// Let's start out by looking for a collision.  If there has been anything seen in
	// the last millisecond, then wait for a random time and check again.

	while (_serialProcess()) {
		unsigned char del;
		del = rand() % 20;
		for (i = 0; i < del; i++) {
			HAL_Delay(1);
			_serialProcess();
		}
		timeout--;
		if (timeout == 0) {
			// Failed to transmit!!!
			return 0;
		}
	}

	HAL_GPIO_WritePin(TEN_GPIO_Port, TEN_Pin, SET);

	// Start of header by writing multiple SOH
	i = SOH;
	for(uint8_t w=0; w<MY_RS485_SOH_COUNT; w++) {
        	HAL_UART_Transmit(_huart, (uint8_t*)&i, 1, 20);
	}
    HAL_UART_Transmit(_huart, (uint8_t*)&data->destination, 1, 20);
	cs += data->destination;
	i = MY_NODE_ID;
    HAL_UART_Transmit(_huart, (uint8_t*)&i, 1, 20);
	cs += MY_NODE_ID;
	i = ICSC_SYS_PACK;  // Command code
    HAL_UART_Transmit(_huart, (uint8_t*)&i, 1, 20);
	cs += ICSC_SYS_PACK;
	len = (data->version_length >> 3) + V2_MYS_HEADER_SIZE;
    HAL_UART_Transmit(_huart, (uint8_t*)&len, 1, 20);
	cs += len;
	i = STX;
    HAL_UART_Transmit(_huart, (uint8_t*)&i, 1, 20);
    HAL_UART_Transmit(_huart, (uint8_t*)datap, len, len + 20);
	for(i=0; i<len; i++) {
		cs += datap[i];
	}
	i = ETX;
    HAL_UART_Transmit(_huart, (uint8_t*)&i, 1, 20);
    HAL_UART_Transmit(_huart, (uint8_t*)&cs, 1, 20);
	i = EOT;
    HAL_UART_Transmit(_huart, (uint8_t*)&i, 1, 20);

	HAL_GPIO_WritePin(TEN_GPIO_Port, TEN_Pin, RESET);
	return 1;
}

void transportInitialise(UART_HandleTypeDef *huart)
{
	_huart = huart;
	HAL_GPIO_WritePin(TEN_GPIO_Port, TEN_Pin, RESET);
	_serialReset();
	_byte_received = 0;
	srand(MY_NODE_ID);
	HAL_Delay(MY_NODE_ID * 50);
	present_node();
}

void transportProcess(void)
{
	_serialProcess();
	if (transportReceive(&_msg))
		transportProcessMessage();
}

uint8_t transportReceive(void* data)
{
	if (_packet_received) {
		memcpy(data,_data,_packet_len);
		_packet_received = 0;
		return _packet_len;
	} else {
		return (0);
	}
}