//----------------------------------------------------
/**
 \file		ctk/inc/modbusA.c
 \ingroup	serial
 \ingroup	all

 \brief		Implementace protokolu SerialModbus (v1.1b3)
 <<< ALL >>>

 \author		ConTeK s.r.o., Bc. Jakub Kastner
 \version	01.01
 \date		05.01.2016
 */
//----------------------------------------------------

#include <modbusA.h>
#include <hwboard.h>
#include "appiodef.h"
#include "gvariables.h"
#include "misc.h"

uint16_t getInpReg(uint8_t _addr)
{
	uint16_t ret = 0;
	switch (_addr)
	{
	case 0:
		ret = sGetDInpByte(1) << 8 | sGetDInpByte(0);
		break;
	case 1:
		ret = sGetAInp(AI_THERMOCOUPLE);
		break;
	case 2:
		ret = sGetAInp(AI_CONNTEMP);
		break;
	case 3:
		ret = sGetAInp(AI_LAMBDA_CURRENT);
		break;
	case 4:
		ret = sGetAInp(AI_RESERVED);
		break;
	case 5:
		ret = sGetAInp(AI_LAMBDA);
		break;
	case 6:
		ret = sGetAInp(AI_PHOTORES);
		break;
	case 7:
		ret = sGetAInp(AI_PHOTOTRANS);
		break;
	default:
		ret = 0;
	}

	return ret;
}

int16_t getHoldReg(uint8_t _addr)
{
	int16_t ret = 0;
	switch (_addr)
	{
	case 0:
		ret = sGetDInpByte(1) << 8 | sGetDInpByte(0);
		break;
	case 1:
		ret = sGetDOutpByte(1) << 8 | sGetDOutpByte(0);
		break;
	case 2:
		ret = gParam.State.ComComand.GLOW;
		ret |= gParam.State.ComComand.BLOW << 1;
		break;
	case 3:
		ret = floor(gParam.State.Temp.TempCouple * 10);
		break;
	case 4:
		ret = floor(gParam.State.Temp.TempJunction * 10);
		break;
	case 5:
		ret = floor(gParam.State.Lambda.LambdaCurrent * 100);
		break;
	case 6:
		ret = floor(gParam.State.Lambda.LambdaVolt * 10);
		break;
	case 7:
		ret = floor(gParam.State.Photo.RES * 10);
		break;
	case 8:
		ret = floor(gParam.State.Photo.TRANS * 10);
		break;
	case 9:
		ret = gParam.State.ComComand.STATUS;
		break;
	default:
		ret = 0;
	}
	return ret;
}

bool_t readInpRegisters(MODBUS_CTX * const _ctx, uint16_t _startAddr,
		uint16_t _quantity, uint8_t *_resp)
{
	int16_t tmp;
	for (uint8_t i = 0; i < _quantity; i++)
	{
		tmp = getInpReg(_startAddr + i);

		_resp[i * 2] = (uint8_t)(tmp >> 8);
		_resp[i * 2 + 1] = (uint8_t)(tmp);
	}
	return TRUE;
}

//----------------------------------------------------
/**
 \brief	Funkce pro formatovani odpovedi  na fci Read_inp_reg

 \param	_ctx		Struktura kontextu automatu komunikace
 \param	_msg		Prijata zprava
 \param  _rply		zprava ktera se ma odeslat jako odpoved

 \return	_ret		Je potreba odpovidat.
 */
//----------------------------------------------------

bool_t BMProcessREAD_INP_REG(MODBUS_CTX * const _ctx, MODBUS_MSG * const _msg,
		MODBUS_MSG * const _rply)
{
	bool_t ret = FALSE;
	uint16_t startAddr = _msg->Data[0] << 8 | _msg->Data[1];
	uint16_t quantity = _msg->Data[2] << 8 | _msg->Data[3];
	uint8_t resp[MODBUS_MAX_DATA_LENGHT];

	if ((1 <= quantity) && (quantity <= 0x7D))
	{
		if (readInpRegisters(_ctx, startAddr, quantity, resp))
		{
			if (_msg->Addr != MODBUS_BROADCAST_ADDRESS)
			{
				_rply->Function = _msg->Function;
				_rply->DataSize = quantity * 2;
				memcpy(_rply->Data, resp, _rply->DataSize);
				ret = TRUE;
			}
		}
		else
		{
			_rply->Function = _msg->Function + 0x80;
			_rply->Data[0] = Server_device_failure;
			_rply->DataSize = 1;
			ret = TRUE;
		}

	}
	else
	{
		_rply->Function = _msg->Function + 0x80;
		_rply->Data[0] = Illegal_data_value;
		_rply->DataSize = 1;
		ret = TRUE;
	}
	return ret;
}

bool_t readHoldRegisters(MODBUS_CTX * const _ctx, uint16_t _startAddr,
		uint16_t _quantity, uint8_t *_resp)
{
	uint16_t tmp;
	for (uint8_t i = 0; i < _quantity; i++)
	{
		tmp = getHoldReg(_startAddr + i);

		_resp[i * 2] = (uint8_t)(tmp >> 8);
		_resp[i * 2 + 1] = (uint8_t)(tmp);
	}
	return TRUE;
}

bool_t BMProcessREAD_holding_reg(MODBUS_CTX * const _ctx,
		MODBUS_MSG * const _msg, MODBUS_MSG * const _rply)
{
	bool_t ret = FALSE;
	uint16_t startAddr = _msg->Data[0] << 8 | _msg->Data[1];
	uint16_t quantity = _msg->Data[2] << 8 | _msg->Data[3];
	uint8_t resp[MODBUS_MAX_DATA_LENGHT];

	if ((1 <= quantity) && (quantity <= 0x7D))
	{
		if (readHoldRegisters(_ctx, startAddr, quantity, resp))
		{
			if (_msg->Addr != MODBUS_BROADCAST_ADDRESS)
			{
				_rply->Function = _msg->Function;
				_rply->DataSize = quantity * 2 + 1;
				_rply->Data[0] = quantity * 2;
				memcpy(_rply->Data + 1, resp, _rply->Data[0]);
				ret = TRUE;
			}
		}
		else
		{
			_rply->Function = _msg->Function + 0x80;
			_rply->Data[0] = Server_device_failure;
			_rply->DataSize = 1;
			ret = TRUE;
		}

	}
	else
	{
		_rply->Function = _msg->Function + 0x80;
		_rply->Data[0] = Illegal_data_value;
		_rply->DataSize = 1;
		ret = TRUE;
	}
	return ret;
}

bool_t WriteSingleReg(MODBUS_CTX * const _ctx, uint16_t _addr, uint16_t _val,
		uint8_t *_resp)
{
	bool_t ret = FALSE;
	switch (_addr)
	{
	case 1:
		gParam.Debug.Out.DO0 = (bool_t) (_val & 0x0001);
		gParam.Debug.Out.DO1 = (bool_t) (_val & 0x0002);
		gParam.Debug.Out.DO2 = (bool_t) (_val & 0x0004);
		gParam.Debug.Out.DO3 = (bool_t) (_val & 0x0008);
		gParam.Debug.Out.DO4 = (bool_t) (_val & 0x0010);
		gParam.Debug.Out.DO5 = (bool_t) (_val & 0x0020);
		gParam.Debug.Out.DO6 = (bool_t) (_val & 0x0040);
		gParam.Debug.Out.DO7 = (bool_t) (_val & 0x0080);

		gParam.Debug.Out.DO8 = (bool_t) (_val & 0x0100);
		gParam.Debug.Out.DO9 = (bool_t) (_val & 0x0200);
		ret = TRUE;
		break;
	case 2:
		gParam.State.ComComand.GLOW = (bool_t) (_val & 0x0001);
		gParam.State.ComComand.BLOW = (bool_t) (_val & 0x0002);
		gParam.State.ComComand.LAM_POWER = (bool_t) (_val & 0x0004);
		ret = TRUE;
		break;
	case 0:
	case 3:
	case 4:
	case 5:
	case 6:
	case 7:
	case 8:
	case 9:
		ret = TRUE;
	default:
		ret = FALSE;
	}
	return ret;
}

bool_t BMProcessWrite_single_reg(MODBUS_CTX * const _ctx,
		MODBUS_MSG * const _msg, MODBUS_MSG * const _rply)
{
	bool_t ret = FALSE;
	uint16_t Addr = _msg->Data[0] << 8 | _msg->Data[1];
	uint16_t Val = _msg->Data[2] << 8 | _msg->Data[3];
	uint8_t resp[MODBUS_MAX_DATA_LENGHT];

	if ((0 <= Addr) && (Addr <= 0xFFFF))
	{
		if (WriteSingleReg(_ctx, Addr, Val, resp))
		{
			if (_msg->Addr != MODBUS_BROADCAST_ADDRESS)
			{
				_rply->Function = _msg->Function;
				memcpy(_rply->Data, _msg->Data, _msg->DataSize);
				ret = TRUE;
			}
		}
		else
		{
			_rply->Function = _msg->Function + 0x80;
			_rply->Data[0] = Server_device_failure;
			_rply->DataSize = 1;
			ret = TRUE;
		}

	}
	else
	{
		_rply->Function = _msg->Function + 0x80;
		_rply->Data[0] = Illegal_data_value;
		_rply->DataSize = 1;
		ret = TRUE;
	}
	return ret;
}

bool_t BMProcessWrite_multi_reg(MODBUS_CTX * const _ctx,
		MODBUS_MSG * const _msg, MODBUS_MSG * const _rply)
{
	bool_t ret = FALSE;
	uint16_t Addr = _msg->Data[0] << 8 | _msg->Data[1];
	uint16_t Val = 0;
	uint16_t quantity = _msg->Data[2] << 8 | _msg->Data[3];
	uint8_t BytesCount = _msg->Data[4];
	uint8_t resp[MODBUS_MAX_DATA_LENGHT];
	bool_t serverFail = FALSE;
	uint8_t i = 0;

	if (((0 < quantity) && (quantity <= 0x7B)) && (quantity * 2 == BytesCount))
	{
		while ((i < quantity) & (!serverFail))
		{
			Val = _msg->Data[5+i] << 8 | _msg->Data[6+i];
			serverFail = ~WriteSingleReg(_ctx, Addr + i, Val, resp);
			i++;
		}

		if (serverFail)
		{
			_rply->Function = _msg->Function + 0x80;
			_rply->Data[0] = Server_device_failure;
			_rply->DataSize = 1;
			ret = TRUE;
		}
		else
		{
			if (_msg->Addr != MODBUS_BROADCAST_ADDRESS)
			{
				_rply->Function = _msg->Function;
				memcpy(_rply->Data, _msg->Data, 4);
				ret = TRUE;
			}
		}

	}
	else
	{
		_rply->Function = _msg->Function + 0x80;
		_rply->Data[0] = Illegal_data_value;
		_rply->DataSize = 1;
		ret = TRUE;
	}
	return ret;
}

//----------------------------------------------------
/**
 \brief	Funkce pro formatovani odpovedi

 \param	_ctx		Struktura kontextu automatu komunikace
 \param	_msg		Prijata zprava

 \return	_ret		Je potreba odpovidat.
 */
//----------------------------------------------------
bool_t ProcesMSG(MODBUS_CTX * const _ctx, MODBUS_MSG * const _msg)
{
	bool_t ret = FALSE;
	MODBUS_MSG reply;

	//Zpracovani adresy
	switch (_msg->Function)
	{
	case Read_inp_reg:
		ret = BMProcessREAD_INP_REG(_ctx, _msg, &reply);
		break;
	case Read_holding_reg:
		ret = BMProcessREAD_holding_reg(_ctx, _msg, &reply);
		break;
	case Write_single_reg:
		ret = BMProcessWrite_single_reg(_ctx, _msg, &reply);
		break;
	case Write_Multi_reg:
		ret = BMProcessWrite_multi_reg(_ctx, _msg, &reply);
		break;
	default://
		reply.Function = _msg->Function + 0x80;
		reply.Data[0] = Illegal_function;
		reply.DataSize = 1;
		ret = TRUE;
	}

	if (ret)
		_ctx->SendMsg(_ctx, &reply);

	return ret;
}

//----------------------------------------------------
/**
 \brief	Implicitni callback volany pri pozadavku na povoleni a zakaz budice vysilace.

 \param	_cmm	Ukazatel na knihovnu
 \param	_enable	= 1, zacatek vysilani, vystupni budic povolit
 = 0, konec vysilani, vystupni budic zakazat
 */
//----------------------------------------------------

void SendCtrl(const COMMDESC * const _cmm, const bool_t _enable)
{
	UNUSED_ALWAYS(_cmm);
	UNUSED_ALWAYS(_enable);
#ifndef MSVC

	if (_enable)
		PIOA->PIO_SODR = PIO_PA24;
	else
		PIOA->PIO_CODR = PIO_PA24;

#endif
	// zde nastavovat nebo nulovat Tx (RS485)
}

//----------------------------------------------------
/**
 \brief	Funkce pro inicializaci komunikacniho automatu

 \param	_ctx		Struktura kontextu automatu komunikace
 \param	_cmm		Adresa knihovny fyzicke vrstvy
 */
//----------------------------------------------------

bool_t MBLAInitialize(MODBUS_LA_CTX * const _ctx, MODBUS_CTX * const _cmm)
{
	//Vynulovani Kontextu
	memset(_ctx, 0, sizeof(MODBUS_LA_CTX));

	flock_t lock = FastLock();

	//donastaveni nizsi vrstvy
	_ctx->Cmm = _cmm;

	_cmm->Cmm->cbSendCtrl = &SendCtrl;

	_cmm->Cmm->CmmA1 = _ctx;

	_cmm->ProcesMsg = &ProcesMSG;

#ifndef MSVC
	//inicializace sts pinu
	volatile Pio * pPIOA = PIOA;
	pPIOA->PIO_WPMR = PIO_KEY;

	pPIOA->PIO_PER = PIO_PA24;
	pPIOA->PIO_OER = PIO_PA24;
	pPIOA->PIO_CODR = PIO_PA24;

#endif

	FastUnlock(lock);

	return TRUE;
}

//----------------------------------------------------
/**
 \brief	Funkce pro ukonceni komunikacniho automatu

 \param	_ctx		Struktura kontextu automatu komunikace
 */
//----------------------------------------------------

void MBLAFinalize(MODBUS_LA_CTX * const _ctx)
{
}
