/*
 * rtu_parser.c
 *
 *  Created on: 27.08.2020
 *      Author: mateusz
 */

#include "./modbus_rtu/rtu_parser.h"
#include "./modbus_rtu/rtu_return_values.h"

#define MODBUS_RTU_MIN_03_04_RESP_LN 	7	// one register to read

/**
* 2021-03-21 03:06:56 CET: SR6WXP>AKLPRZ,WIDE2-1,qAS,SQ6NDL:=5038.00N101747.00E# DigiW1 + WX ~ Lotnisko Polska Nowa Wies ~ https://www.facebook.com/aeroklub.opolski
* 2021-03-21 03:06:58 CET: SR6WXP>AKLPRZ,WIDE2-1,qAR,SR9NSK:T#237,023,004,001,000,122,10000110
* 2021-03-21 03:07:58 CET: SR6WXP>AKLPRZ,WIDE2-1,qAO,SR6NKB:>F1V FFED, F2V 47, F3V 0, F4V 0, F5V 0, F6V 0
* 2021-03-21 03:08:59 CET: SR6WXP>AKLPRZ,WIDE2-1,qAR,SR9NSK:!5038.00N/01747.00E_344/006g011t025r...p...P...b09952h71
* 2021-03-21 03:11:59 CET: SR6WXP>AKLPRZ,WIDE2-1,qAO,SR6NKB:>F1V FFEE, F2V 47, F3V 0, F4V 0, F5V 0, F6V 0
* 2021-03-21 03:13:00 CET: SR6WXP>AKLPRZ,WIDE2-1,qAO,SR6NKB:!5038.00N/01747.00E_352/005g009t025r...p...P...b09951h71
* 2021-03-21 03:16:00 CET: SR6WXP>AKLPRZ,WIDE2-1,qAO,SR6NKB:>F1V FFEE, F2V 47, F3V 0, F4V 0, F5V 0, F6V 0
* 2021-03-21 03:17:01 CET: SR6WXP>AKLPRZ,WIDE2-1,qAR,SR9NSK:!5038.00N/01747.00E_348/006g008t025r...p...P...b09950h71
* 
* 2021-03-21 05:36:27 CET: SR6WXP>AKLPRZ,WIDE2-1,qAS,SQ6NDL:>F1V FFFA, F2V 46, F3V 0, F4V 0, F5V 0, F6V 0
* 2021-03-21 05:37:28 CET: SR6WXP>AKLPRZ,WIDE2-1,qAS,SQ6NDL:!5038.00N/01747.00E_339/003g007t027r...p...P...b09923h70
* 2021-03-21 05:37:30 CET: SR6WXP>AKLPRZ,WIDE2-1,qAR,SR9NSK:T#252,026,004,001,000,122,10000110
* 2021-03-21 05:40:30 CET: SR6WXP>AKLPRZ,WIDE2-1,qAS,SQ6NDL:>F1V FFFA, F2V 46, F3V 0, F4V 0, F5V 0, F6V 0
*/

int32_t rtu_parser_03_04_registers(uint8_t* input, uint16_t input_ln, rtu_register_data_t* output, rtu_exception_t* exception) {
	uint32_t retval = MODBUS_RET_UNINITIALIZED;

	uint16_t data = 0;

	uint8_t slave_address_from_frame = 0;

	// iterator through input table and registers table
	int j = 3;

	// rewind the input buffer if the first byte is not valid modbus rtu slave address
	if (*input < 1 || *input > 0xF7) {
		// don't loop here and assume that the modbus response is shifted only
		// by one byte
		input++;
	}

	// fetch slave address
	data = *input;

	// TODO: store slave address
	slave_address_from_frame = data;

	// fetch function code
	data = *(input + 1);

	// if the exception flag is set
	if ((data & 0x80) > 0) {
		// get error code
		data = *(input + 2);

		// parse the exception value
		*exception = rtu_exception_from_frame_data(data);

		// and set the return value
		retval = MODBUS_RET_GOT_EXCEPTION;
	}

	// check if the function code is correct or not
	else if (data == 0x03 || data == 0x04) {

		// check if this is an answer from the slave we expect
		if (slave_address_from_frame == output->slave_address) {
			// fetch the function result lenght in bytes
			data = *(input + 2);

			// store amount of registers in this response
			output->number_of_registers = data / 2;

			// get all registers values
			for (int i = 0; i < output->number_of_registers && i < MODBUS_RTU_MAX_REGISTERS_AT_ONCE; i++) {
				output->registers_values[i] = *(input + j) << 8 | *(input + j + 1);

				// moving to next 16bit word with next register
				j += 2;
			}
			retval = MODBUS_RET_OK;
		}
		else {
			retval = MODBUS_RET_UNEXP_SLAVE_ADR;
		}

	}
	else {
		// if not exit with an error as this isn't
		// correct parser for this modbus function
		retval = MODBUS_RET_WRONG_FUNCTION;
	}

	return retval;
}

