/*
 * 2021/04/23　T.Wanibe　Retouch
 * 20191029 T.Wanibe 保持メモリを個別に持たないとうまくゆかない
 * 個別に4つのメモリを保持するように変更
 * 20191028 T.Wanibe 一般的なModbusデバイスとして構築出来るように
 * MgsModbusをレタッチすることにした。『ModbusPoll』とかLabVIEWの
 * Modbusライブラリを元に調査したところ、MgsModbusはスレーブサーバ
 * としてちゃんと機能するライブラリだと云う事が確認出来た。
 * STM32duinoとしてコンパイル出来ないライブラリが殆ど。ライブラリ側で
 * Arduinoしか意識していない為と思われる。
 * MgsModbusはシンプルであるが故にSTM32duinoでもコンパイルが通り実行
 * 可能となる。
 * が、アドレスマップがゼロベースであったり、4つのブロックの内Coilブロック
 * しかうまくアクセス出来ない点も確認出来た。
 * 一般的なModbusデバイスとして認識出来るように大胆なデバッグを実施する
 * ことにした。
 * まず、Localな扱いとした。inoと同じフォルダに配置すること。
 * Slave機能のコードのみ残して(Masterのコードは削除)
 * 
 * MgsModbus.h-Modbus TCPマスターおよびスレーブ用のArduinoライブラリ。
 * V-0.1.1著作権（C）2013 Marco Gerritse
 * Arduino 1.0で作成およびテスト済み
 * 
 * このライブラリは、
 *  MgsModbus.h-Modbus TCPマスターおよびスレーブ用のArduinoライブラリ。
 * を原点としています。
 * 
 * テストに使用される外部ソフトウェア：
 * ★Modbus Poll 
 *      Modbus Poll x64　version 9.1.0 Build 1286, self-installing
 * ★LabVIEW Modbusライブラリ     
 *      NI Modbus Library v1.1.5.39 by National Instruments
 * ??
 * このライブラリは、すべてのmodbusデータ（mbData []配列）に単一のメモリブロック
 * を使用します。
 * 16ビットアクセスまたはアクセスビットを介して、いくつかのmodbus機能を介して同じ
 * データに到達できます。 MbDataの長さは少なくとも1でなければなりません。
 * ●スレーブの場合、次のmodbus機能が実装されます：1、2、3、4、5、6、15、16
 * ●内部および外部アドレスは1ベースに修正しています。
 * ●EtherNetライブラリは『Ethernet3』を採用しています。W5500を安定して使用する為です。
 */
#include        "Arduino.h"
#define         WIZFI360_EVB_PICO       1
#if     defined(WIZFI360_EVB_PICO)
        #include        "WizFi360.h"
#else
        #include        <SPI.h>
        #include        <Ethernet3.h>
#endif

#ifndef MgsModbus_h
#define MgsModbus_h
#define MbCoilDataLen           4       // length of the MbCoilDataLen array (Equivalent16bit*2)
#define MbDiscreteDataLen       4       // length of the MbDiscreteDataLen array(Equivalent16bit*2)
#define MbHoldDataLen           64      // length of the MbHoldDataLen array
#define MbInputDataLen          96      // length of the MbInputDataLen array
#define MB_PORT                 502
enum MB_FC {
        MB_FC_NONE                     = 0,
        MB_FC_READ_COILS               = 1,
        MB_FC_READ_DISCRETE_INPUT      = 2,
        MB_FC_READ_REGISTERS           = 3,
        MB_FC_READ_INPUT_REGISTER      = 4,
        MB_FC_WRITE_COIL               = 5,
        MB_FC_WRITE_REGISTER           = 6,
        MB_FC_WRITE_MULTIPLE_COILS     = 15,
        MB_FC_WRITE_MULTIPLE_REGISTERS = 16
};
enum MB_XC {
        MB_XC_NONE                      = 0,
        MB_XC_Illegal_Function          = 1,
        MB_XC_Illegal_Data_Address      = 2,
        MB_XC_Illegal_Data_Value        = 3,
        MB_XC_Slave_Device_Failure      = 4,
        MB_XC_Acknowledge               = 5,
        MB_XC_Slave_Device_Busy         = 6,
        MB_XC_Negative_Acknowledge      = 7,
        MB_XC_Memory_Parity_Error       = 8,
        MB_XC_Gateway_Path_Unavailable  = 10,
        MB_XC_Gateway_Target_Failed     = 11
};
class MgsModbus
{
        public:
                // general
                MgsModbus();
                word MbCoilData[MbCoilDataLen];                 // memory block that holds all the modbus user data
                word MbDiscreteData[MbDiscreteDataLen];         // memory block that holds all the modbus user data
                word MbHoldData[MbHoldDataLen];                 // memory block that holds all the modbus user data
                word MbInputData[MbInputDataLen];               // memory block that holds all the modbus user data                
                boolean GetBit(MB_FC fc,word Number);
                boolean SetBit(MB_FC fc,word Number,boolean Data);       // returns true when the number is in the MbData range
                // modbus master
                //void Req(MB_FC FC, word Ref, word Count, word Pos);
                //void MbmRun();
                //IPAddress remSlaveIP;
                // modbus slave
                bool MbsRun();  
                word GetDataLen();
        private: 
                // general
                MB_FC SetFC(int fc);
                // modbus master
                //uint8_t MbmByteArray[260];                    // send and recieve buffer
                //MB_FC MbmFC;
                //int MbmCounter;
                //void MbmProcess();
                //word MbmPos;
                //word MbmBitCount;
                //modbus slave
                uint8_t MbsByteArray[260];                      // send and recieve buffer
                MB_FC MbsFC;
                MB_XC MbsXC;
};
#endif

/* 
 *  2021/04/23　T.Wanibe　Retouch
 */
#include "MgsModbus.h"
//
#if     defined(WIZFI360_EVB_PICO)
        WiFiServer      MbServer(MB_PORT);
        WiFiClient      MbmClient;
#else
        EthernetServer  MbServer(MB_PORT);
        EthernetClient  MbmClient;
#endif
#define DEBUG      0                    //　1にするとRESPONS:00 00 00 00 00 00 00 d0 01 という出力がある
MgsModbus::MgsModbus()
{
}
char buf[32];
//****************** Recieve data for ModBusSlave ****************
//      2021/3/5　接続が有った場合はtrueをない場合はfalseを返すように変更
bool MgsModbus::MbsRun()
{ 
        bool    flagConnected   = false;
        MbsXC                   = MB_XC_NONE;
        //****************** Read from socket ****************
        //EthernetClient client   = MbServer.available();
#if     defined(WIZFI360_EVB_PICO)
        WiFiClient      client  =       MbServer.available();
        //Serial.print(F("\nWiFiClient"));
#else
        EthernetClient  client  =       MbServer.available();
        //Serial.print(F("\nEthernetClient"));
#endif
        delay(1);
        flagConnected           =       client.available();
        if(flagConnected){
                digitalWrite(LED_BUILTIN, LOW);
                delay(1);
                int i = 0;
                while(client.available()){
                        MbsByteArray[i] = client.read();
                        i++;
                }
                MbsFC = SetFC(MbsByteArray[7]);  //Byte 7 of request is FC
#if DEBUG
                Serial.print(F("\nREQUEST:"));
                for(int j = 0;j<i;j++){
                        sprintf(buf,"%02x ",MbsByteArray[j]);
                        Serial.print(buf);
                }
                Serial.println(F(""));
                digitalWrite(LED_BUILTIN, HIGH);
#endif                
        }
        int Start, WordDataLength, ByteDataLength, CoilDataLength, MessageLength;
        //SwitchCaseに変更しようとしたがコンパイルエラーが発生するので諦めた
        if(MbsFC == MB_FC_READ_COILS) {                                                 //****************** Read Coils (1) **********************
                Start           = word(MbsByteArray[8],MbsByteArray[9]);                //StartAddress確認
                CoilDataLength  = word(MbsByteArray[10],MbsByteArray[11]);              //データ長の確認
#if DEBUG
                sprintf(buf,"01StartADRS=%d Length=%d",Start,CoilDataLength);
                Serial.println(buf);
#endif  
                if((Start<0) || ((CoilDataLength + Start) > (MbCoilDataLen * 16))){
                        //Exception MB_XC_Illegal_Data_Address
                        MbsXC           = MB_XC_Illegal_Data_Address;
                        MbsByteArray[7] = 0x10 + MbsByteArray[7];
                        MbsByteArray[8] = MbsXC;
                        MessageLength = 9;
                }else{
                        ByteDataLength  = CoilDataLength / 8;
                        if(ByteDataLength * 8 < CoilDataLength) ByteDataLength++;      
                        CoilDataLength  = ByteDataLength * 8;
                        MbsByteArray[5] = ByteDataLength + 3;                           //Number of bytes after this one.
                        MbsByteArray[8] = ByteDataLength;                               //Number of bytes after this one (or number of bytes of data).
                        for(int i = 0; i < ByteDataLength ; i++){
                                MbsByteArray[9 + i] = 0;                                // To get all remaining not written bits zero
                                for(int j = 0; j < 8; j++){
                                        bitWrite(MbsByteArray[9 + i], j, GetBit(MB_FC_READ_COILS,Start + i * 8 + j));
                                }
                        }
                        MessageLength = ByteDataLength + 9;
                }      
                //
        }else if(MbsFC == MB_FC_READ_DISCRETE_INPUT) {                                  //****************** Read Coils (2) **********************
                Start           = word(MbsByteArray[8],MbsByteArray[9]);                //StartAddress確認
                CoilDataLength  = word(MbsByteArray[10],MbsByteArray[11]);              //データ長の確認
#if DEBUG
                sprintf(buf,"02StartADRS=%d Length=%d",Start,CoilDataLength);
                Serial.println(buf);
#endif
                if((Start<0) || ((CoilDataLength + Start) > (MbDiscreteDataLen * 16))){
                        //Exception MB_XC_Illegal_Data_Address
                        MbsXC           = MB_XC_Illegal_Data_Address;
                        MbsByteArray[7] = 0x10 + MbsByteArray[7];
                        MbsByteArray[8] = MbsXC;
                        MessageLength = 9;
                }else{
                        ByteDataLength  = CoilDataLength / 8;
                        if(ByteDataLength * 8 < CoilDataLength) ByteDataLength++;      
                        CoilDataLength  = ByteDataLength * 8;
                        MbsByteArray[5] = ByteDataLength + 3;                           //Number of bytes after this one.
                        MbsByteArray[8] = ByteDataLength;                               //Number of bytes after this one (or number of bytes of data).
                        for(int i = 0; i < ByteDataLength ; i++){
                                MbsByteArray[9 + i] = 0;                                // To get all remaining not written bits zero
                                for(int j = 0; j < 8; j++){
                                        bitWrite(MbsByteArray[9 + i], j, GetBit(MB_FC_READ_DISCRETE_INPUT,Start + i * 8 + j));
                                }
                        }
                        MessageLength = ByteDataLength + 9;
                }
                //
        }else if(MbsFC == MB_FC_READ_REGISTERS) {                                       //****************** Read Registers (3) ******************
                Start           = word(MbsByteArray[8],MbsByteArray[9]);
                WordDataLength  = word(MbsByteArray[10],MbsByteArray[11]);
                if((Start<0) || ((WordDataLength + Start) > MbInputDataLen)){
                        //Exception MB_XC_Illegal_Data_Address
                        MbsXC           = MB_XC_Illegal_Data_Address;
                        MbsByteArray[7] = 0x10 + MbsByteArray[7];
                        MbsByteArray[8] = MbsXC;
                        MessageLength = 9;
                }else{
                        ByteDataLength  = WordDataLength * 2;
                        MbsByteArray[5] = ByteDataLength + 3;                           //Number of bytes after this one.
                        MbsByteArray[8] = ByteDataLength;                               //Number of bytes after this one (or number of bytes of data).
                        for(int i = 0; i < WordDataLength; i++){
                                MbsByteArray[ 9 + i * 2] =      highByte(MbHoldData[Start + i]);
                                MbsByteArray[10 + i * 2] =      lowByte(MbHoldData[Start + i]);
                        }
                        MessageLength = ByteDataLength + 9;
                }
                //
        }else if(MbsFC == MB_FC_READ_INPUT_REGISTER) {                                  //****************** Read Registers (4) ******************
                Start           = word(MbsByteArray[8],MbsByteArray[9]);
                WordDataLength  = word(MbsByteArray[10],MbsByteArray[11]);
                if((Start<0) || ((WordDataLength + Start) > MbHoldDataLen)){
                        //Exception MB_XC_Illegal_Data_Address
                        MbsXC           = MB_XC_Illegal_Data_Address;
                        MbsByteArray[7] = 0x10 + MbsByteArray[7];
                        MbsByteArray[8] = MbsXC;
                        MessageLength = 9;
                }else{
                        ByteDataLength  = WordDataLength * 2;
                        MbsByteArray[5] = ByteDataLength + 3;                           //Number of bytes after this one.
                        MbsByteArray[8] = ByteDataLength;                               //Number of bytes after this one (or number of bytes of data).
                        for(int i = 0; i < WordDataLength; i++){
                                MbsByteArray[ 9 + i * 2] =      highByte(MbInputData[Start + i]);
                                MbsByteArray[10 + i * 2] =      lowByte(MbInputData[Start + i]);
                        }
                        MessageLength = ByteDataLength + 9;
                }
                //
        }else if(MbsFC == MB_FC_WRITE_COIL) {                                           //****************** Write Coil (5) **********************
                Start           = word(MbsByteArray[8],MbsByteArray[9]);
#if DEBUG
                sprintf(buf,"05StartADRS=%d",Start);
                Serial.println(buf);
#endif  
                if((Start<0) || (Start > (MbCoilDataLen * 16))){
                        //Exception MB_XC_Illegal_Data_Address
                        MbsXC           = MB_XC_Illegal_Data_Address;
                        MbsByteArray[7] = 0x10 + MbsByteArray[7];
                        MbsByteArray[8] = MbsXC;
                        MessageLength   = 9;
                }else{
                        if (word(MbsByteArray[10],MbsByteArray[11]) == 0xFF00){SetBit(MB_FC_WRITE_COIL,Start,true);}
                        if (word(MbsByteArray[10],MbsByteArray[11]) == 0x0000){SetBit(MB_FC_WRITE_COIL,Start,false);}
                        MbsByteArray[5] = 2;                                            //Number of bytes after this one.
                        MessageLength   = 8;
                }
                //
        }else if(MbsFC == MB_FC_WRITE_REGISTER) {                                       //****************** Write Register (6) ******************
                Start                   = word(MbsByteArray[8],MbsByteArray[9]);
                if((Start<0) || (Start > MbHoldDataLen)){
                        //Exception MB_XC_Illegal_Data_Address
                        MbsXC           = MB_XC_Illegal_Data_Address;
                        MbsByteArray[7] = 0x10 + MbsByteArray[7];
                        MbsByteArray[8] = MbsXC;
                        MessageLength = 9;
                }else{
                        MbHoldData[Start]       = word(MbsByteArray[10],MbsByteArray[11]);
                        MbsByteArray[5]         = 6;                                    //Number of bytes after this one.
                        MessageLength           = 12;
                }
                //
        }else if(MbsFC == MB_FC_WRITE_MULTIPLE_COILS) {                                 //****************** Write Multiple Coils (15) **********************
                Start           = word(MbsByteArray[8],MbsByteArray[9]);
                CoilDataLength  = word(MbsByteArray[10],MbsByteArray[11]);              //データ長の確認
                if((Start<0) || ((CoilDataLength + Start) > (MbCoilDataLen * 16))){
                        //Exception MB_XC_Illegal_Data_Address
                        MbsXC           = MB_XC_Illegal_Data_Address;
                        MbsByteArray[7] = 0x10 + MbsByteArray[7];
                        MbsByteArray[8] = MbsXC;
                        MessageLength   = 9;
                }else{
                        MbsByteArray[5] = 6;
                        for(int i = 0; i < CoilDataLength; i++){
                                SetBit(MB_FC_WRITE_MULTIPLE_COILS,Start + i,bitRead(MbsByteArray[13 + (i/8)],i-((i/8)*8)));
                        }
                        MessageLength = 12;
                }
                //
        }else if(MbsFC == MB_FC_WRITE_MULTIPLE_REGISTERS) {                             //****************** Write Multiple Registers (16) ******************
                Start                   = word(MbsByteArray[8],MbsByteArray[9]);
                WordDataLength          = word(MbsByteArray[10],MbsByteArray[11]);
                if((Start<0) || ((WordDataLength + Start) > MbHoldDataLen)){
                        //Exception MB_XC_Illegal_Data_Address
                        MbsXC           = MB_XC_Illegal_Data_Address;
                        MbsByteArray[7] = 0x10 + MbsByteArray[7];
                        MbsByteArray[8] = MbsXC;
                        MessageLength = 9;
                }else{
                        ByteDataLength  = WordDataLength * 2;
                        MbsByteArray[5] = 6;
                        for(int i = 0; i < WordDataLength; i++){
                                MbHoldData[Start + i] =  word(MbsByteArray[ 13 + i * 2],MbsByteArray[14 + i * 2]);
                        }
                        MessageLength = 12;
                }
        }else{
                //Exception 01:IllegalFunction
                MbsXC           = MB_XC_Illegal_Function;
                ByteDataLength  = 0;
                MbsByteArray[7] = 0x10 + MbsByteArray[7];
                MbsByteArray[8] = MbsXC;
                MessageLength = 9 + ByteDataLength;
        }
        //
        MbsFC = MB_FC_NONE;
        client.write(MbsByteArray, MessageLength);
#if DEBUG
        Serial.print(F("\nRESPONS:"));
        for(int i = 0;i<MessageLength;i++){
                sprintf(buf,"%02x ",MbsByteArray[i]);
                Serial.print(buf);
        }
        Serial.println(F(""));
#endif 
        return flagConnected;
}
//****************** ?? ****************** 20210423 retouch
MB_FC MgsModbus::SetFC(int fc){
        MB_FC FC;
        switch(fc){
                case 1:
                        FC = MB_FC_READ_COILS;
                        break;
                case 2:
                        FC = MB_FC_READ_DISCRETE_INPUT;
                        break;
                case 3:
                        FC = MB_FC_READ_REGISTERS;
                        break;
                case 4:
                        FC = MB_FC_READ_INPUT_REGISTER;
                        break;
                case 5:
                        FC = MB_FC_WRITE_COIL;
                        break;
                case 6:
                        FC = MB_FC_WRITE_REGISTER;
                        break;
                case 15:
                        FC = MB_FC_WRITE_MULTIPLE_COILS;
                        break;
                case 16:
                        FC = MB_FC_WRITE_MULTIPLE_REGISTERS;
                        break;
                default:
                        FC = MB_FC_NONE;
        }
        return FC;
}
//---------------------
word MgsModbus::GetDataLen(){
        int MbDataLen;
        switch(MbsFC){
                case MB_FC_READ_COILS:
                case MB_FC_WRITE_COIL:
                case MB_FC_WRITE_MULTIPLE_COILS:
                        MbDataLen = MbCoilDataLen;
                        break;
                case MB_FC_READ_DISCRETE_INPUT:
                        MbDataLen = MbDiscreteDataLen;
                        break;
                case MB_FC_READ_REGISTERS:
                case MB_FC_WRITE_REGISTER:
                case MB_FC_WRITE_MULTIPLE_REGISTERS:
                        MbDataLen = MbHoldDataLen;
                        break;
                case MB_FC_READ_INPUT_REGISTER:
                        MbDataLen = MbInputDataLen;
                        break;
                default:
                        MbDataLen = 0;
        }
        return MbDataLen;
}
//---------------------
boolean MgsModbus::GetBit(MB_FC fc,word Number){
        boolean Tmp;
        int ArrayPos = Number / 16;
        int BitPos = Number - ArrayPos * 16;
        if(fc == MB_FC_READ_DISCRETE_INPUT){
                Tmp = bitRead(MbDiscreteData[ArrayPos],BitPos);
        }else{
                Tmp = bitRead(MbCoilData[ArrayPos],BitPos);
        }
        return Tmp;
}
//---------------------
boolean MgsModbus::SetBit(MB_FC fc,word Number,boolean Data){
        boolean Overrun;
        int ArrayPos = Number / 16;
        int BitPos = Number - ArrayPos * 16;
        if(fc == MB_FC_READ_DISCRETE_INPUT){
                Overrun = ArrayPos > MbDiscreteDataLen * 16;                            // check for data overrun
                if (!Overrun){                 
                        bitWrite(MbDiscreteData[ArrayPos],BitPos,Data);
                }
        }else{
                Overrun = ArrayPos > MbCoilDataLen * 16;                                // check for data overrun
                if (!Overrun){                 
                        bitWrite(MbCoilData[ArrayPos],BitPos,Data);
                }                        
        }
        return Overrun;
}