#include "ModbusRTUSlave.h"
#include "utility/LinkedList.h"


ModbusRTUSlave::ModbusRTUSlave(HardwareSerial* serialport, const uint8_t controlPinArg)
  : ser(serialport)
  , controlPin(controlPinArg)
  , isReading(true)
  , rWords(new wordsList())
  , rBits(new bitsList())
  , rwWords(new wordsList())
  , rwBits(new bitsList())
{
  pinMode(controlPin, OUTPUT);
  digitalWrite(controlPin, LOW);
}

void ModbusRTUSlave::begin(byte address, uint32_t baudrate) 
{
	if (connected) {
		ser->end();
	}
	slaveAddress = address;
	ser->begin(baudrate);
	ResCnt=0;
	connected = true;
}

void ModbusRTUSlave::end()
{
	if (connected) {
		ser->end();
		connected = false;
	}
}

bool ModbusRTUSlave::addCoilArea(u16 address, u8* values, int cnt)
{
	if(getBitAddress(rwBits, address)==NULL)
	{
		rwBits->add(new ModbusRTUSlaveBitAddress(address, values, cnt));
		return true;
	}
	return false;
}

bool ModbusRTUSlave::addDiscreteInputArea(u16 address, u8* values, int cnt)
{
	if(getBitAddress(rBits, address)==NULL)
	{
		rBits->add(new ModbusRTUSlaveBitAddress(address, values, cnt));
		return true;
	}
	return false;
}

bool ModbusRTUSlave::addHoldingRegisterArea(u16 address, u16* values, int cnt)
{
	if(getWordAddress(rwWords, address)==NULL)
	{
		rwWords->add(new ModbusRTUSlaveWordAddress(address, values, cnt));
		return true;
	}
	return false;
}

bool ModbusRTUSlave::addInputRegisterArea(u16 address, u16* values, int cnt)
{
	if(getWordAddress(rWords, address)==NULL)
	{
		rWords->add(new ModbusRTUSlaveWordAddress(address, values, cnt));
		return true;
	}
	return false;
}

// bool ModbusRTUSlave::addValidator(u16 reg, u16 min, u16 max, u16 step=5)
// {
// 	return validator.add(reg, min, max, step);
// }

ModbusRTUSlaveWordAddress* ModbusRTUSlave::getWordAddress(wordsList *words, u16 Addr)
{
	ModbusRTUSlaveWordAddress* ret=NULL;
	for(int i = 0; i < words->size(); i++)
	{
		ModbusRTUSlaveWordAddress* a = words->get(i);
		if(a!=NULL && Addr >= a->addr && Addr < a->addr + a->len) ret=a;
	}
	return ret;
}
ModbusRTUSlaveBitAddress* ModbusRTUSlave::getBitAddress(bitsList *bits, u16 Addr)
{
	ModbusRTUSlaveBitAddress* ret=NULL;
	for(int i = 0; i < bits->size(); i++)
	{
		ModbusRTUSlaveBitAddress* a = bits->get(i);
		if(a!=NULL && Addr >= a->addr && Addr < a->addr + (a->len*8)) ret=a;
	}
	return ret;
}

ModbusRTUSlaveWordAddress* ModbusRTUSlave::getWordAddress(wordsList *words, u16 Addr, u16 Len)
{
	ModbusRTUSlaveWordAddress* ret=NULL;
	for(int i = 0; i < words->size(); i++)
	{
		ModbusRTUSlaveWordAddress* a = words->get(i);
		if(a!=NULL && Addr >= a->addr && Addr+Len <= a->addr + a->len) ret=a;
	}
	return ret;
}
ModbusRTUSlaveBitAddress* ModbusRTUSlave::getBitAddress(bitsList *bits, u16 Addr, u16 Len)
{
	ModbusRTUSlaveBitAddress* ret=NULL;
	for(int i = 0; i < bits->size(); i++)
	{
		ModbusRTUSlaveBitAddress* a = bits->get(i);
		if(a!=NULL && Addr >= a->addr && Addr+Len <= a->addr + (a->len*8)) ret=a;
	}
	return ret;
}


void ModbusRTUSlave::process()
{
	bool bvalid = true;
	while(isDataAvail()) 
	{
		byte d = doRead();

		lstResponse[ResCnt++]=d;
		if(ResCnt>=4)
		{
			requestedSlave = lstResponse[0];
			if(requestedSlave == slaveAddress)
			{
				fnCode = lstResponse[1];
	            requestedRegister = (lstResponse[2] << 8) | lstResponse[3];
				receivedLastRequest = micros();
				switch(fnCode)
				{
					case 1: // read coils
						if(ResCnt >= 8)
						{
							readCoilOrDiscreteInput(rwBits, bvalid);
						}
						break;
					case 2: // read discrete inputs
						if(ResCnt >= 8)
						{
							readCoilOrDiscreteInput(rBits, bvalid);
						}
						break;
					case 3: // read holding registers
						if(ResCnt >= 8)
						{
							readHoldingOrInputRegister(rwWords, bvalid);
						}
						break;
					case 4: // read input registers
						if(ResCnt >= 8)
						{
							readHoldingOrInputRegister(rWords, bvalid);
						}
						break;
					case 5: // write single coil
						if(ResCnt >= 8)
						{
							u16 Data = (lstResponse[4] << 8) | lstResponse[5];
							byte hi = 0xFF, lo = 0xFF;
							getCRC(lstResponse,300, 0, 6, &hi, &lo);
							ModbusRTUSlaveBitAddress *a = getBitAddress(rwBits, requestedRegister);
							if (a != NULL && lstResponse[6] == hi && lstResponse[7] == lo)
							{
								u16 stidx = (requestedRegister - a->addr) / 8;

								bool b = Data==0xFF00;
								callbackCoil(requestedRegister, b);
								bitWrite(a->values[stidx], (requestedRegister - a->addr)%8, b);

								byte ret[8];
								ret[0]=requestedSlave;
								ret[1]=fnCode;
								ret[2]=((requestedRegister&0xFF00)>>8);
								ret[3]=((requestedRegister&0x00FF));
								ret[4]=((Data&0xFF00)>>8);
								ret[5]=((Data&0x00FF));
  							    byte hi = 0xFF, lo = 0xFF;
								getCRC(ret, 8, 0, 6, &hi, &lo);
								ret[6]=hi;
								ret[7]=lo;
								doWrite(ret, 8);
								
								ResCnt=0;
							}
							else bvalid = false;
						}
						break;
					case 6: // write single register
						if(ResCnt >= 8)
						{
							u16 Data = (lstResponse[4] << 8) | lstResponse[5];
							byte hi = 0xFF, lo = 0xFF;
							getCRC(lstResponse,300, 0, 6, &hi, &lo);
							ModbusRTUSlaveWordAddress *a = getWordAddress(rwWords, requestedRegister);
							if (a != NULL && lstResponse[6] == hi && lstResponse[7] == lo)
							{
								u16 stidx = requestedRegister - a->addr;

								a->values[stidx] = callbackRegister(requestedRegister, Data);

								byte ret[8];
								ret[0]=requestedSlave;	
								ret[1]=fnCode;	
								ret[2]=((requestedRegister&0xFF00)>>8);
								ret[3]=((requestedRegister&0x00FF));
								ret[4]=((Data&0xFF00)>>8);
								ret[5]=((Data&0x00FF));
  							    byte hi = 0xFF, lo = 0xFF;
								getCRC(ret, 8, 0, 6, &hi, &lo);
								ret[6]=hi;
								ret[7]=lo;
								doWrite(ret, 8);

								ResCnt=0;
							}
							else bvalid = false;
						}
						break;
					case 15: // write multiple coils
						if(ResCnt >= 7)
						{
							u16 Length = (lstResponse[4] << 8) | lstResponse[5];
							u8 ByteCount = lstResponse[6];
							if(ResCnt >= 9+ByteCount)
							{
								byte hi = 0xFF, lo = 0xFF;
								getCRC(lstResponse,300, 0, 7 + ByteCount, &hi, &lo);
								if(lstResponse[(9 + ByteCount - 2)] == hi && lstResponse[(9 + ByteCount - 1)] == lo)
								{
									ModbusRTUSlaveBitAddress *a = getBitAddress(rwBits, requestedRegister, Length);
									if (a != NULL) 
									{
										bool b;
										u16 offset = 0;
										u16 stidx = (requestedRegister - a->addr) / 8;
										int ng=(requestedRegister - a->addr) % 8;
										int ns=stidx;

										for(int i=7; i<7+ByteCount;i++)
										{
											byte val = lstResponse[i];
											for(int j=0;j<8;j++)
											{
												b = bitRead(val,j);
												callbackCoil(requestedRegister + offset, b);
												bitWrite(a->values[ns], ng++, b);
												if(ng==8){ns++;ng=0;}
												offset++;
											}
										}

										if(bvalid)
										{
											byte ret[8];
											ret[0]=requestedSlave;	
											ret[1]=fnCode;	
											ret[2]=((requestedRegister&0xFF00)>>8);
											ret[3]=((requestedRegister&0x00FF));
											ret[4]=((Length&0xFF00)>>8);
											ret[5]=((Length&0x00FF));
											byte hi = 0xFF, lo = 0xFF;
											getCRC(ret, 8, 0, 6, &hi, &lo);
											ret[6]=hi;
											ret[7]=lo;
											doWrite(ret, 8);

											ResCnt=0;
										}
									}
								}
								else bvalid=false;
							}
						}
						break; 
					case 16: // write multiple registers
						if(ResCnt >= 7)
						{
							u16 Length = (lstResponse[4] << 8) | lstResponse[5];
							u8 ByteCount = lstResponse[6];
							if(ResCnt >= 9+ByteCount)
							{
								byte hi = 0xFF, lo = 0xFF;
								getCRC(lstResponse,300, 0, 7 + ByteCount, &hi, &lo);
								if(lstResponse[(9 + ByteCount - 2)] == hi && lstResponse[(9 + ByteCount - 1)] == lo)
								{
									for(int i=7; i<7+ByteCount;i+=2)
									{
										u16 data = callbackRegister(requestedRegister, lstResponse[i] << 8 | lstResponse[i+1]);
										ModbusRTUSlaveWordAddress *a = getWordAddress(rwWords, requestedRegister + ((i-7)/2));
										if (a != NULL) { a->values[(requestedRegister + ((i-7)/2)) - a->addr] = data;	}
										else { bvalid=false; break; }
									}
									if(bvalid)
									{
										byte ret[8];
										ret[0]=requestedSlave;	
										ret[1]=fnCode;	
										ret[2]=((requestedRegister&0xFF00)>>8);
										ret[3]=((requestedRegister&0x00FF));
										ret[4]=((Length&0xFF00)>>8);
										ret[5]=((Length&0x00FF));
										byte hi = 0xFF, lo = 0xFF;
										getCRC(ret, 8, 0, 6, &hi, &lo);
										ret[6]=hi;
										ret[7]=lo;
										doWrite(ret, 8);

										ResCnt=0;
									}
								}
								else bvalid=false;
							}
						}
						break;
				}
			}
			else bvalid = false;
		}
		lastrecv = millis();
	}
	if(!bvalid && ResCnt>0) ResCnt=0;
	if(ResCnt>0 && (millis()-lastrecv > 200 || millis() < lastrecv)) ResCnt=0;
}
/*
void ModbusRTUSlave::getCRC(LinkedList<byte>* pby, int startindex, int nSize, byte* byFirstReturn, byte* bySecondReturn)
{
	int uIndex;
	byte uchCRCHi = 0xff;
	byte uchCRCLo = 0xff;
	for (int i = startindex; i < startindex + nSize && i<pby->size(); i++)
	{
		uIndex = uchCRCHi ^ pby->get(i);
		uchCRCHi = uchCRCLo ^ auchCRCHi[uIndex];
		uchCRCLo = auchCRCLo[uIndex];
	}
	(*byFirstReturn) = uchCRCHi;
	(*bySecondReturn) = uchCRCLo;
}
*/
void ModbusRTUSlave::getCRC(byte* pby, int arsize, int startindex, int nSize, byte* byFirstReturn, byte* bySecondReturn)
{
	int uIndex;
	byte uchCRCHi = 0xff;
	byte uchCRCLo = 0xff;
	for (int i = startindex; i < startindex + nSize && i<arsize; i++)
	{
		uIndex = uchCRCHi ^ pby[i];
		uchCRCHi = uchCRCLo ^ auchCRCHi[uIndex];
		uchCRCLo = auchCRCLo[uIndex];
	}
	(*byFirstReturn) = uchCRCHi;
	(*bySecondReturn) = uchCRCLo;
}

void ModbusRTUSlave::switchToReadingIfNotReadingNow()
{
  if (!isReading)
	{
	  ser->flush();
	  digitalWrite(controlPin, LOW);
	  isReading = true;
	}
}

bool ModbusRTUSlave::isDataAvail()
{
  switchToReadingIfNotReadingNow();
  return ser->available();
}

int ModbusRTUSlave::doRead()
{
  switchToReadingIfNotReadingNow();
  return ser->read();
}

void ModbusRTUSlave::doWrite(byte* buffer, int const length)
{
	if (isReading)
	{
		digitalWrite(controlPin, HIGH);
		isReading = false;
	}
	if (waitWithAnswerMicroS) {
		unsigned long t = micros() - receivedLastRequest;
		if (t < waitWithAnswerMicroS)
			delayMicroseconds(waitWithAnswerMicroS - t);
	}
	ser->write(buffer, length);
}

void ModbusRTUSlave::readCoilOrDiscreteInput(bitsList *bits, bool &valid)
{
	u16 Length = (lstResponse[4] << 8) | lstResponse[5];
	byte hi = 0xFF, lo = 0xFF;
	getCRC(lstResponse,300, 0, 6, &hi, &lo);
	ModbusRTUSlaveBitAddress *a = getBitAddress(bits, requestedRegister, Length);
	if (Length > 0 && a != NULL && lstResponse[6] == hi && lstResponse[7] == lo)
	{
		u16 stidx = (requestedRegister - a->addr) / 8;
		u16 nlen = ((Length-1) / 8)+1;

		byte dat[nlen];
		memset(dat,0,nlen);

		int ng=(requestedRegister - a->addr) % 8;
		int ns=stidx;
		for(int i=0;i<nlen;i++)
		{
			byte val=0;
			for(int j=0;j<8;j++)
			{
				if(bitRead(a->values[ns], ng++)) bitSet(val,j);
				if(ng==8){ns++;ng=0;}
			}
			dat[i]=val;
		}
		
		byte ret[3+nlen+2];
		ret[0]=requestedSlave;
		ret[1]=fnCode;
		ret[2]=nlen;
		for(int i=0;i<nlen;i++) ret[3+i]=dat[i];
		byte hi = 0xFF, lo = 0xFF;
		getCRC(ret, 3+nlen+2, 0, 3+nlen, &hi, &lo);
		ret[3+nlen]=hi;
		ret[3+nlen+1]=lo;
		doWrite(ret, 3+nlen+2);

		ResCnt=0;
	}
	else valid = false;
}

void ModbusRTUSlave::readHoldingOrInputRegister(wordsList *words, bool &valid)
{
	u16 Length = (lstResponse[4] << 8) | lstResponse[5];
	byte hi = 0xFF, lo = 0xFF;
	getCRC(lstResponse,300, 0, 6, &hi, &lo);
	ModbusRTUSlaveWordAddress *a = getWordAddress(words, requestedRegister, Length);
	if (Length > 0 && a != NULL && lstResponse[6] == hi && lstResponse[7] == lo)
	{
		u16 stidx = requestedRegister - a->addr;
		u16 nlen = Length * 2;

		byte ret[3+nlen+2];
		ret[0]=requestedSlave;
		ret[1]=fnCode;
		ret[2]=nlen;
		for(int i=stidx;i<stidx+Length;i++)
		{
			ret[3+((i-stidx)*2)+0]=((a->values[i] & 0xFF00) >> 8);
			ret[3+((i-stidx)*2)+1]=((a->values[i] & 0xFF));
		}
		byte hi = 0xFF, lo = 0xFF;
		getCRC(ret, 3+nlen+2, 0, 3+nlen, &hi, &lo);
		ret[3+nlen]=hi;
		ret[3+nlen+1]=lo;
		doWrite(ret, 3+nlen+2);

		ResCnt=0;
	}
	else valid = false;
}


bool getBit(u8* area, int index)
{
	u16 stidx = index / 8;
	return bitRead(area[stidx], index%8);
}

void setBit(u8* area, int index, bool value)
{
	u16 stidx = index / 8;
	bitWrite(area[stidx], index%8, value);
}

ModbusRTUSlaveBitAddress::ModbusRTUSlaveBitAddress(u16 address, u8* value, int cnt)
{
	addr = address;
	values = value;
	len = cnt;
}

ModbusRTUSlaveWordAddress::ModbusRTUSlaveWordAddress(u16 address, u16* value, int cnt)
{
	addr = address;
	values = value;
	len = cnt;
}