/*
   UDP syntax:
   signals:
     DS18B20 1wire sensor packet:    rail1 1w 2864fc3008082 25.44
     DS2438 1wire sensor packet:     rail1 1w 2612c3102004f 25.44 1.23 0.12
     digital input state:            rail1 di1 1
     analog input state:             rail1 ai1 250
   commands:
     relay on command:               rail1 do12 on
     relay off command:              rail1 do5 off
     high side switch on command:    rail1 ho2 on    
     high side switch off command:   rail1 ho4 off
     low side switch on command:     rail1 lo1 on    
     low side switch off command:    rail1 lo2 off
     analog output command:          rail1 ao1 180
     status command:                 rail1 stat10
     reset command:                  rail1 rst
   default scan cycles:
     1wire cycle:                    30000 ms
     analog input cycle:             10000 ms
     heart beat cycle(only RS485):   60000 ms

   MODBUS TCP commands: FC1 - FC16
   MODBUS RTU commands: FC3, FC6, FC16
   
   MODBUS register map (1 register = 2 bytes = 16 bits)
          register number            description
          0                          relay outputs 1-8
          1                          relay outputs 9-12
          2                          digital outputs HSS 1-4, LSS 1-4
          3                          analog output 1
          4                          analog output 2
          5                          digital inputs 1-8
          6                          digital inputs 9-16
          7                          digital inputs 17-24
          8                          analog input 1           0-256
          9                          analog input 2           0-256
          10                         service - reset (bit 0)
          11                         1st DS2438 Temp (value multiplied by 100)
          12                         1st DS2438 Vad (value multiplied by 100)
          13                         1st DS2438 Vsens (value multiplied by 100)
          -
          39                         DS2438 values (up to 10 sensors) 
          40-50                      DS18B20 Temperature (up to 10 sensors) (value multiplied by 100)
   
   Combination of 1wire sensors must be up to 10pcs maximum (DS18B20 or DS2438)
   
   using RS485 the UDP syntax must have newline \n symbol at the end of the command line
     
*/

#define OK	0
#if 1
#define dbg(x...) Serial.print(x)
//#define dbg(x...) Serial.println(x)
#define dbgln(x...) Serial.println(x)
#else
#define dbg(x...) 
#define dbgln(x...) 
#endif

#define ver 	3


#include <SimpleModbusSlave.h>
#include <OneWire.h>
#include <DS2438.h>
#include <Ethernet.h>
#include <EthernetUdp.h>
#include <MgsModbus.h>


byte mac[] = {0xDE, 0xAD, 0xBE, 0xEF, 0xFE, 0};
unsigned int listenPort = 44444;
unsigned int sendPort = 55555;
unsigned int remPort = 55556;
IPAddress listenIpAddress;
IPAddress sendIpAddress(255, 255, 255, 255);

#define relOut1Byte 0
#define relOut2Byte 1
#define hssLssByte 2
#define anaOut1Byte 3
#define anaOut2Byte 4
#define digInp1Byte 5
#define digInp2Byte 6
#define digInp3Byte 7
#define anaInp1Byte 8
#define anaInp2Byte 9
#define serviceByte 10
#define oneWireTempByte 11
#define oneWireVadByte 12
#define oneWireVsensByte 13
#define oneWireDS18B20Byte 40

#define inputPacketBufferSize UDP_TX_PACKET_MAX_SIZE
//char inputPacketBuffer[UDP_TX_PACKET_MAX_SIZE];
char inputPacketBuffer[UDP_TX_PACKET_MAX_SIZE];
#define outputPacketBufferSize 100
char outputPacketBuffer[outputPacketBufferSize];

EthernetUDP udpRecv;
EthernetUDP udpSend;

#define oneWireCycle 5000
#define oneWireSubCycle 5000
#define anaInputCycle 10000
#define heartBeatCycle 60000
#define statusLedTimeOn 50
#define statusLedTimeOff 990
#define commLedTimeOn 50
#define commLedTimeOff 50
#define debouncingTime 5
#define Serial1TxControl 16
#define numOfRelays 14
int relayPins[numOfRelays] = {43, 44, 45, 46, 47, 48, 49, 2, 3, 4, 5, 40, 41, 42};
#define numOfHSSwitches 0
int HSSwitchPins[numOfHSSwitches] = {};
#define numOfLSSwitches 0
int LSSwitchPins[numOfLSSwitches] = {};
#define numOfAnaOuts 8
int anaOutPins[numOfAnaOuts] = {9,8 ,7 ,6 ,13 ,12 ,11 ,10};
#define numOfAnaInputs 8
int analogPins[numOfAnaInputs] = {57, 56 ,55 ,54 ,61 ,60 ,59 ,58};
float analogStatus[numOfAnaInputs];
#define numOfDigInputs 16
int inputPins[numOfDigInputs] = {22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32, 33, 34, 35, 36, 37};
int inputStatus[numOfDigInputs];
int inputStatusNew[numOfDigInputs];
int inputChangeTimestamp[numOfDigInputs];
#define numOfDipSwitchPins 5
int dipSwitchPins[numOfDipSwitchPins] = {66, 65, 64, 63, 67};
#define numOfLedPins 2
int ledPins[numOfLedPins] = {69, 68};
int boardAddress = 0;
int ethOn = 0;
int rtuOn = 0;
long baudRate = 115200;
int Serial1TxDelay = 10;
int Serial1TimeOut = 20;
bool ticTac = 0;


String boardAddressStr;
String boardAddressRailStr;
String railStr = "rail";
String digInputStr = "di";
String anaInputStr = "ai";
String relayStr = "ro";
String HSSwitchStr = "ho";
String LSSwitchStr = "lo";
String anaOutStr = "ao";
String digStatStr = "stat";
String rstStr = "rst";
String heartBeatStr = "hb";
String relayOnCommands[numOfRelays];
String relayOffCommands[numOfRelays];
String HSSwitchOnCommands[numOfHSSwitches];
String HSSwitchOffCommands[numOfHSSwitches];
String LSSwitchOnCommands[numOfLSSwitches];
String LSSwitchOffCommands[numOfLSSwitches];

String digStatCommand[numOfDigInputs];
String anaOutCommand[numOfAnaOuts];

#if 0
//  https://www.maximintegrated.com/en/design/technical-documents/app-notes/4/4104.html
class MyOneWire: public OneWire {
byte MyOneWire::write_bit_(byte lo, byte rec, signed char ovd, byte hi) {
// timing if rec< 4:  lo+rec+rec+ovd+hi
// timing if rec>=4:  lo+rec+rec/2+ovd+hi
// timing if ovd is negative and >=4: lo+rec+hi-ovd/2
// timing if ovd is negative and < 4: lo+rec+hi-ovd
// rec get decremented, ovd if negative replaced rec
        IO_REG_TYPE mask IO_REG_MASK_ATTR = bitmask;
        volatile IO_REG_TYPE *reg IO_REG_BASE_ATTR = baseReg;
                noInterrupts();
                DIRECT_WRITE_LOW(reg, mask);
                DIRECT_MODE_OUTPUT(reg, mask);  
                delayMicroseconds(lo);			// LOW
                DIRECT_MODE_INPUT(reg, mask);  
                DIRECT_WRITE_HIGH(reg, mask);   	
                delayMicroseconds(1);			// pull-up and wait sample time offset
        	r = DIRECT_READ(reg, mask);
                delayMicroseconds(--rec);		// 3µs to 10µs -- recovery time
	if(ovd>0) {
                DIRECT_MODE_OUTPUT(reg, mask);  
                delayMicroseconds(ovd); 		// 8µs to 15µs 	-- overdrive
	} else if(ovd<0) rec=-ovd;
                DIRECT_MODE_INPUT(reg, mask); 
                interrupts();
                delayMicroseconds(hi);			// -- high time -- pull-up
                DIRECT_MODE_OUTPUT(reg, mask);  
		if(++rec&4) rec>>=1;			// adjust timing ???
                delayMicroseconds(rec);	// 3µs to 10µs	-- recovery time overdrive (for read before release)
                DIRECT_MODE_INPUT(reg, mask); 
		return r;
}
void MyOneWire::write_bit(uint8_t v)
{
        if (v & 1) {
		write_bit_(7,3,2,65); // 70
        } else {
		write_bit_(49,6,10,2); // 70
        }
}
uint8_t MyOneWire::read_bit(void)
{
		return write_bit_(1,1,-3,62); // 70
}
void MyOneWire::write_bytes(const uint8_t *buf, uint16_t count, bool power /* = 0 */) {
  for (uint16_t i = 0 ; i < count ; i++)
    write(buf[i],1);	// don't disable pull-up during byte write. !!!
  if (!power) {
    noInterrupts();
    DIRECT_MODE_INPUT(baseReg, bitmask);
    DIRECT_WRITE_LOW(baseReg, bitmask);
    interrupts();
  }
}
#define OneWire MyOneWire
#endif

class Timer {
  private:
    unsigned long timestampLastHitMs;
    unsigned long sleepTimeMs;
  public:
    boolean isOver();
    void sleep(unsigned long sleepTimeMs);
};

boolean Timer::isOver() {
    if (millis() - timestampLastHitMs < sleepTimeMs) {
        return false;
    }
    timestampLastHitMs = millis();
    return true;
}

void Timer::sleep(unsigned long sleepTimeMs) {
    this->sleepTimeMs = sleepTimeMs;
    timestampLastHitMs = millis();
}

Timer statusLedTimerOn;
Timer statusLedTimerOff;
Timer oneWireTimer;
Timer oneWireSubTimer;
Timer analogTimer;
Timer heartBeatTimer;

MgsModbus Mb;

////////////////////////////////
/////  1 Wire configuration
////////////////////////////////

#define OW_resolution   11 								// DS2438
#define OW_maxSensors   10 								// per pin
#define OW_retry   	0 								// for 1W search and CRC-ERROR
#define OW_pins		38,39								

/******************************************************************************/
/*********** 1 Wire defines, don't modify unless you know what doing **********/
/******************************************************************************/

OneWire ds; DS2438 ds2438(&ds);
#define OW_count	(sizeof(ow)/sizeof(ow[0]))				// number of pins
#define OW_counts	(sizeof(ow_sensors[0])/sizeof(ow_sensors[0][0]))	// number of sensors
byte ow[] = { OW_pins };
word ow_count[OW_count];			// hi byte = DS18x20, LO byte = DS2438	
byte ow_sensors[OW_count][OW_maxSensors][8];	// layout DS2438 from end down and DS18x20 from 0 up


/******************************************************************************/
/******************************************************************************/
////////////////////////////////////////////////////////////////////////////////
/******************************************************************************/
/*********** -- CODE -- don't modify unless you know what doing ---  **********/
/******************************************************************************/
/******************************************************************************/

String oneWireAddressToString(byte addr[], byte id=-1);

void setup() {
    Serial.begin(9600);  
    

    dbg("Railduino firmware version: ");
    dbgln(ver);
    
    for (int i = 0; i < numOfDigInputs; i++) {
        pinMode(inputPins[i], INPUT);
        inputStatus[i] = 1;
        inputStatusNew[i] = 0;
        digStatCommand[i] = digStatStr + String(i + 1, DEC);
    }
    
    for (int i = 0; i < numOfRelays; i++) {
        pinMode(relayPins[i], OUTPUT);
        relayOnCommands[i] = relayStr + String(i + 1, DEC) + " on";
        relayOffCommands[i] = relayStr + String(i + 1, DEC) + " off";
        setRelay(i, 0);
    }

    for (int i = 0; i < numOfHSSwitches; i++) {
        pinMode(HSSwitchPins[i], OUTPUT);
        HSSwitchOnCommands[i] = HSSwitchStr + String(i + 1, DEC) + " on";
        HSSwitchOffCommands[i] = HSSwitchStr + String(i + 1, DEC) + " off";
        setHSSwitch(i, 0);
    }

    for (int i = 0; i < numOfLSSwitches; i++) {
        pinMode(LSSwitchPins[i], OUTPUT);
        LSSwitchOnCommands[i] = LSSwitchStr + String(i + 1, DEC) + " on";
        LSSwitchOffCommands[i] = LSSwitchStr + String(i + 1, DEC) + " off";
        setLSSwitch(i, 0);
    }
   
    for (int i = 0; i < numOfAnaOuts; i++) {
        pinMode(anaOutPins[i], OUTPUT);
        anaOutCommand[i] = anaOutStr + String(i + 1, DEC);
        setAnaOut(i, 0);
    }

    for (int i = 0; i < numOfAnaInputs; i++) {
        pinMode(analogPins[i], INPUT);
    }

    for (int i = 0; i < numOfLedPins; i++) {
        pinMode(ledPins[i], OUTPUT);
    }

    analogTimer.sleep(anaInputCycle);
    statusLedTimerOn.sleep(statusLedTimeOn);
    statusLedTimerOff.sleep(statusLedTimeOff);
    heartBeatTimer.sleep(heartBeatCycle);
 
    for (int i = 0; i < 4; i++) {
        pinMode(dipSwitchPins[i], INPUT);
        if (!digitalRead(dipSwitchPins[i])) { boardAddress |= (1 << i); }
    }

  Ethernet.init(53);  // Most Arduino shields
    pinMode(dipSwitchPins[4], INPUT);
    if (!digitalRead(dipSwitchPins[4]))  { ethOn = 1; dbgln("Ethernet ON");} else { ethOn = 0; dbgln("Ethernet OFF");}

    pinMode(dipSwitchPins[4], INPUT); 
    if (!digitalRead(dipSwitchPins[4]))  { rtuOn = 0; dbgln("485 RTU ON ");} else { rtuOn = 1; dbgln("485 RTU OFF");}
    
    dbg(baudRate);
    dbg(" Bd, Tx Delay: ");
    dbg(Serial1TxDelay);
    dbg(" ms, Timeout: ");
    dbg(Serial1TimeOut);
    dbgln(" ms");
            
    boardAddressStr = String(boardAddress);  
    boardAddressRailStr = railStr + String(boardAddress);

    if (ethOn) {
      mac[5] = (0xED + boardAddress);
      listenIpAddress = IPAddress(192, 168, 178, 150 + boardAddress);
      if (Ethernet.begin(mac) == 0)
      {
        dbgln("Failed to configure Ethernet using DHCP, using Static Mode");
        Ethernet.begin(mac, listenIpAddress);
      }
         
      udpRecv.begin(listenPort);
      udpSend.begin(sendPort);

      dbg("IP: ");
      printIPAddress();
      dbgln();
    }


    memset(Mb.MbData, 0, sizeof(Mb.MbData));

    if (rtuOn) {
      modbus_configure(&Serial1, baudRate, SERIAL_8N1, boardAddress, Serial1TxControl, sizeof(Mb.MbData), Mb.MbData); 
     }
    
    Serial1.begin(baudRate);
    Serial1.setTimeout(Serial1TimeOut);
    pinMode(Serial1TxControl, OUTPUT);
    digitalWrite(Serial1TxControl, 0);

    dbg("Address: ");
    dbgln(boardAddressStr);
    
    for(byte i=0;i<=OW_retry;i++) { if(i) delay(400); if(owlookUpSensors()==OK) break; }

}

void loop() {
     
    readDigInputs();

    readAnaInputs();

    processCommands();
    
    processOnewire();

    statusLed();
    
    if (ethOn) {Mb.MbsRun();} else {heartBeat();}

    if (rtuOn) {modbus_update();}   
 
}

void (* resetFunc) (void) = 0; 


void printIPAddress()
{
  for (byte thisByte = 0; thisByte < 4; thisByte++) {
    dbg(Ethernet.localIP()[thisByte]);
    dbg(".");
  }
}

void heartBeat() {
    if (!heartBeatTimer.isOver()) { 
      return;
    }  
    
    heartBeatTimer.sleep(heartBeatCycle);
    if (ticTac) {
      sendMsg(heartBeatStr + " 1");
      ticTac = 0;
    } else {
      sendMsg(heartBeatStr + " 0");
      ticTac = 1;
    }   
}

void statusLed() {
    if (statusLedTimerOff.isOver()) { 
       statusLedTimerOn.sleep(statusLedTimeOn);
       statusLedTimerOff.sleep(statusLedTimeOff);
       digitalWrite(ledPins[0],HIGH);  
    }  
    if (statusLedTimerOn.isOver()) { digitalWrite(ledPins[0],LOW); } 
}

String oneWireAddressToString(byte addr[], byte id) {
    String s = "";
        if(id!=-1) s += String(id, HEX);
    for (int i = 0; i < 8; i++) {
        s += String(addr[i], HEX);
    }
    return s;
}

byte owlookUpSensors(){
  byte sensors[8];
  byte i,j=0,k,l,m,e=0;
  for(i=0;i<OW_count;i++) {
    k=l=m=0;	
    ds.begin(ow[i]);
    if(!ds.reset()) continue; // no 1Wire present;
    ds.reset_search();
    while ((j < OW_counts) && (ds.search(sensors))){
     if (!OneWire::crc8(sensors, 7) != sensors[7]){
        if (sensors[0] == 38){
           for (l=0;l<8;l++){ ow_sensors[i][k][l]=sensors[l]; }  
           k++;
        } else 
        if (sensors[0] == 10||sensors[0] == 28||sensors[0] == 22) { 
           for (l=0,m++;l<8;l++){ ow_sensors[i][OW_counts-m][l]=sensors[l]; }
           dssetresolution(ds,sensors,OW_resolution);
        } else dbg("1-wire sensor unsupported: "),dbgln(oneWireAddressToString(sensors,i));
        j++;
     } else e++, dbg("1-wire sensor crc-error: "),dbg(oneWireAddressToString(sensors,i)),dbgln(OneWire::crc8(sensors, 7),HEX);
    }
    ow_count[i]  = m<<8|k;
    dbg("1-wire sensors found "); dbg(i+1); dbg(": "); dbgln(j);
  }
    if(j) dbg("1-wire sensors: "); dbgln(j);
    if(e) dbg("1-wire crc-errors: "); dbgln(e);
  return e;
}


void dssetresolution(OneWire ow, byte addr[8], byte resolution) {
  byte resbyte = 0x1F;
  if (resolution == 12){ resbyte = 0x7F; }
  else if (resolution == 11) { resbyte = 0x5F; }
  else if (resolution == 10) { resbyte = 0x3F; }

  ow.reset();
  ow.select(addr);
  ow.write(0x4E);         
  ow.write(0);            
  ow.write(0);            
  ow.write(resbyte);      
  ow.write(0x48);         
}

void dsconvertcommand(OneWire ow, byte addr[8]){
  ow.reset();
  ow.select(addr);
  ow.write(0x44,1);       
}

float dsreadtemp(OneWire ow, byte addr[8]) {
  int i;
  byte data[12];
  float celsius;
  ow.reset();
  ow.select(addr);
  ow.write(0xBE);         
  for ( i = 0; i < 9; i++) { 
    data[i] = ow.read();
  }

  int16_t TReading = (data[1] << 8) | data[0];  
  celsius = 0.0625 * TReading;
  return celsius;
}


void processOnewire() {
   static byte state;
   static byte cnt;
   static byte id;
   static byte pos;		// need to be word if total sensors is greater as 84
   
   switch(state)
   {
   case 0:
      if (!oneWireTimer.isOver()) {
         return;  
      }
        for (id = OW_count;id--;)                                               // calc timing for ds2438
                cnt+= (byte)ow_count[id];
        if(oneWireCycle>=(oneWireSubCycle*(word)++cnt))                         // check timing including ds18x20
                oneWireTimer.sleep(oneWireCycle-(oneWireSubCycle*(word)cnt));   // apply timing
        else
                oneWireTimer.sleep(oneWireCycle);                               // or not if timing is bad.

      		oneWireSubTimer.sleep(oneWireSubCycle);				// fire it in case of bad config
      cnt=0;
      state++;
      break;
   case 1:
      if (!oneTimer.isOver()) {
        return;
      }
      if (!oneWireSubTimer.isOver()) {
        return;
      }
      if (id < OW_count)
      if ((cnt < (byte)ow_count[id])){          
      	oneWireSubTimer.sleep(oneWireSubCycle);				// fire it only if 1W present
    	if(!cnt) ds.begin(ow[id]);
         ds2438.begin();
         ds2438.update(ow_sensors[id][OW_counts-++cnt]); 
         if (!ds2438.isError()) {
            if (rtuOn) {
              Mb.MbData[oneWireTempByte + pos]  = ds2438.getTemperature()*100; 
              Mb.MbData[oneWireVadByte + pos]   = ds2438.getVoltage(DS2438_CHA)*100; 
              Mb.MbData[oneWireVsensByte + pos] = ds2438.getVoltage(DS2438_CHB)*100; 
            } else {
              sendMsg("1w " + oneWireAddressToString(ow_sensors[id][OW_counts-cnt],id) + " " + String(ds2438.getTemperature(), 2) + " " + String(ds2438.getVoltage(DS2438_CHA), 2) + " " + String(ds2438.getVoltage(DS2438_CHB), 2));
            }
         }
	 pos+=3;
      } else {
        cnt = 0;
        id++;
      } else {
        cnt = 0;
        id  = 0;
	pos=0;
        state++;
      }
      break;
   case 2:
      if (id < OW_count)
      if (cnt < (ow_count[id]>>8)){  
	    if(!cnt) ds.begin(ow[id]);
         dsconvertcommand(ds,ow_sensors[id][cnt]);            
         cnt++;
      } else {
        cnt = 0;
        id++;
      } else {
        cnt = 0;
        id  = 0;
        state++;
      	oneWireSubTimer.sleep(oneWireSubCycle); 
      }
      break;
   case 3:
      if (!oneWireSubTimer.isOver()) {
         return;
      }
      if (id < OW_count)
      if (cnt < (ow_count[id]>>8)){  
      if(!cnt) ds.begin(ow[id]);
         if (rtuOn) {
           Mb.MbData[oneWireDS18B20Byte + cnt] = dsreadtemp(ds,ow_sensors[cnt])*100; 
         } else {
           sendMsg("1w " + oneWireAddressToString(ow_sensors[id][cnt],id) + " " + String(dsreadtemp(ds,ow_sensors[id][cnt]), 2));
         }
         cnt++;
      } else {
        cnt = 0;
        id++;
      } else {
        cnt = 0;
        id  = 0;
        state = 0;
      }
      break;
   }
}


void readDigInputs() {

    int timestamp = millis();
    for (int i = 0; i < numOfDigInputs; i++) {      
       int oldValue = inputStatus[i];
       int newValue = inputStatusNew[i];
       int curValue = digitalRead(inputPins[i]);
       int byteNo = i/8;
       int bitPos = i - (byteNo*8);
       
       if (oldValue != newValue) {
          if(newValue != curValue) {
             inputStatusNew[i] = curValue;
          } else if(timestamp - inputChangeTimestamp[i] > debouncingTime) {
             inputStatus[i] = newValue;
             if(!newValue) {
                bitWrite(Mb.MbData[byteNo+5], bitPos, 1);
                sendInputOn(i + 1);
             } else {
                bitWrite(Mb.MbData[byteNo+5], bitPos, 0);         
                sendInputOff(i + 1);  
             }
          }
       
       } else {
          if(oldValue != curValue) {
             inputStatusNew[i] = curValue;
             inputChangeTimestamp[i] = timestamp;
          }
       }
    }
}

void readAnaInputs() {
    
    if (!analogTimer.isOver()) {
      return;
    }
    
    analogTimer.sleep(anaInputCycle);
    for (int i = 0; i < numOfAnaInputs; i++) {
        int pin = analogPins[i];
        float value = analogRead(pin) * (255 / 1023.0);
        float oldValue = analogStatus[i];
        analogStatus[i] = value;
        if (value != oldValue) {
            Mb.MbData[i+8] = (byte) value;
            sendAnaInput(i+1,Mb.MbData[i+8]);
        }
    } 
}

void sendInputOn(int input) {
    sendMsg(digInputStr + String(input, DEC) + " 1");
}

void sendInputOff(int input) {
    sendMsg(digInputStr + String(input, DEC) + " 0");
}

void sendAnaInput(int input, float value) {
    sendMsg(anaInputStr + String(input, DEC) + " " + String(value, 2));
}

void sendMsg(String message) {
    message = railStr + boardAddressStr + " " + message;
    message.toCharArray(outputPacketBuffer, outputPacketBufferSize);
   
    digitalWrite(ledPins[1],HIGH);
    
    if (ethOn) {
      udpSend.beginPacket(sendIpAddress, remPort);
      udpSend.write(outputPacketBuffer, message.length());
      udpSend.endPacket();
    }

    if (!rtuOn) {
    digitalWrite(Serial1TxControl, HIGH);     
    Serial1.print(message + "\n");
    delay(Serial1TxDelay);    
    digitalWrite(Serial1TxControl, LOW);
    }
     
    digitalWrite(ledPins[1],LOW);
    
    dbg("Sending packet: ");
    dbgln(message);
}

void setRelay(int relay, int value) {
    if (relay > numOfRelays) {
        return;
    }
    dbgln("Writing to relay " + String(relay+1) + " value " + String(value));
    digitalWrite(relayPins[relay], value);
}

void setHSSwitch(int hsswitch, int value) {
    if (hsswitch > numOfHSSwitches) {
        return;
    }
    dbgln("Writing to high side switch" + String(hsswitch+1) + " value " + String(value));
    digitalWrite(HSSwitchPins[hsswitch], value);
}

void setLSSwitch(int lsswitch, int value) {
    if (lsswitch > numOfLSSwitches) {
        return;
    }
    dbgln("Writing to low side switch" + String(lsswitch+1) + " value " + String(value));
    digitalWrite(LSSwitchPins[lsswitch], value);
}

void setAnaOut(int pwm, int value) {
    if (pwm > numOfAnaOuts) {
        return;
    }
    dbgln("Writing to analog output " + String(pwm+1) + " value " + String(value));
    analogWrite(anaOutPins[pwm], value);
}

boolean receivePacket(String *cmd) {

    if (!rtuOn) {
      while (Serial1.available() > 0) {    
        *cmd = Serial1.readStringUntil('\n'); 
        if (cmd->startsWith(boardAddressRailStr)) {
          cmd->replace(boardAddressRailStr, "");
          cmd->trim();
          return true;
        }   
      }   
    }
    
    if (ethOn) {
      int packetSize = udpRecv.parsePacket();
      if (packetSize) {
        memset(inputPacketBuffer, 0, sizeof(inputPacketBuffer));
        udpRecv.read(inputPacketBuffer, inputPacketBufferSize);
        *cmd = String(inputPacketBuffer);
        if (cmd->startsWith(boardAddressRailStr)) {
            cmd->replace(boardAddressRailStr, "");
            cmd->trim();
            return true;
        }
      }
    }
   
    return false;
}


void processCommands() {
    String cmd;
    byte byteNo, curBitValue, bitPos;
    int curAnaVal;
     
    for (int i = 0; i < numOfRelays; i++) {
       if (i<8) {
         setRelay(i, bitRead(Mb.MbData[relOut1Byte],i));
       } else {
         setRelay(i, bitRead(Mb.MbData[relOut2Byte],i-8));
       }
       
    }
    
    for (int i = 0; i < numOfHSSwitches; i++) {
       setHSSwitch(i, bitRead(Mb.MbData[hssLssByte],i));
    }

    for (int i = 0; i < numOfLSSwitches; i++) {
       setLSSwitch(i, bitRead(Mb.MbData[hssLssByte],i+numOfHSSwitches));
    }

    for (int i = 0; i < numOfAnaOuts; i++) {
       setAnaOut(i, Mb.MbData[anaOut1Byte+i]);
    }

    if (bitRead(Mb.MbData[serviceByte],0)) {
        resetFunc();
    }
  
    if (receivePacket(&cmd)) {
        dbg("Received packet: ");
        dbgln(cmd);
        digitalWrite(ledPins[1],HIGH);
        if (cmd.startsWith(relayStr)) {
            for (int i = 0; i < numOfRelays; i++) {
                if (i<8) {byteNo = relOut1Byte; bitPos = i;} else {byteNo = relOut2Byte; bitPos = i-8;}
                if (cmd == relayOnCommands[i]) {
                    bitWrite(Mb.MbData[byteNo],bitPos,1);
                } else if (cmd == relayOffCommands[i]) {
                    bitWrite(Mb.MbData[byteNo],bitPos,0);
                }
            }
        } else if (cmd.startsWith(HSSwitchStr)) {
            for (int i = 0; i < numOfHSSwitches; i++) {
                if (cmd == HSSwitchOnCommands[i]) {
                    bitWrite(Mb.MbData[hssLssByte],i,1);
                } else if (cmd == HSSwitchOffCommands[i]) {
                    bitWrite(Mb.MbData[hssLssByte],i,0);
                }
            }
        } else if (cmd.startsWith(LSSwitchStr)) {
            for (int i = 0; i < numOfLSSwitches; i++) {
                if (cmd == LSSwitchOnCommands[i]) {
                    bitWrite(Mb.MbData[hssLssByte],i + numOfHSSwitches,1);
                } else if (cmd == LSSwitchOffCommands[i]) {
                    bitWrite(Mb.MbData[hssLssByte],i + numOfHSSwitches,0);
                }
            }
        } else if (cmd.startsWith(anaOutStr)) {
            String anaOutValue = cmd.substring(anaOutStr.length() + 2);
            for (int i = 0; i < numOfAnaOuts; i++) {
                if (cmd.substring(0,anaOutStr.length()+1) == anaOutCommand[i]) {
                    Mb.MbData[anaOut1Byte+i] = anaOutValue.toInt();
                } 
            }
        } else if (cmd.startsWith(rstStr)) {
            bitWrite(Mb.MbData[serviceByte],0,1);
        }
        digitalWrite(ledPins[1],LOW);
      }
 }