#include <WiFi.h>
uint8_t CMD[13] ;//= {0x01, 0x10, 0x00, 0x00, 0x00, 0x02, 0x04, 0x00, 0x01, 0x00, 0x01, 0x63, 0xAF};
#define RXD2 16 // Defualt Rx pin
#define TXD2 17 // Default Tx pin

#define SOFTSERIALBUADRATE 9600
byte *commandRS485(uint8_t id, uint8_t *buffer, size_t size)
{
    // ประกาศและสร้างอาเรย์ data ใหม่โดยมีขนาดของข้อมูล size+5 บายต์
    uint8_t *data = new uint8_t[size + 5];

    // กำหนดค่าเริ่มต้นในอาเรย์ data
    data[0] = 0x01; // ส่วนหัว
    data[1] = id;   // ไอดี
    data[2] = 0x02; // ความยาวข้อมูล
    if (id == 0x65 || id == 0x70)
    {
        data[3] = size + 5;
        // คัดลอกข้อมูลจาก buffer เข้าในอาเรย์ data ตั้งแต่ตำแหน่งที่ 3 เป็นต้นไป
        for (int i = 4; i < size + 4; i++)
        {
            data[i] = buffer[i - 4];
        }

        data[size + 4] = 0x03;     // ส่วนท้าย
    }
    else
    {

        // คัดลอกข้อมูลจาก buffer เข้าในอาเรย์ data ตั้งแต่ตำแหน่งที่ 3 เป็นต้นไป
        for (int i = 3; i < size + 3; i++)
        {
            data[i] = buffer[i - 3];
        }

        data[size + 3] = 0x03;     // ส่วนท้าย
        data[size + 4] = size + 5; // ความยาวทั้งหมด
    }

    // ส่งคืนอาเรย์ data ที่สร้างขึ้น
    return data;
}
uint16_t mosbusCRC16(uint8_t *nData, uint16_t wLength) {
  static const uint16_t wCRCTable[] = {
    0X0000, 0XC0C1, 0XC181, 0X0140, 0XC301, 0X03C0, 0X0280, 0XC241,
    0XC601, 0X06C0, 0X0780, 0XC741, 0X0500, 0XC5C1, 0XC481, 0X0440,
    0XCC01, 0X0CC0, 0X0D80, 0XCD41, 0X0F00, 0XCFC1, 0XCE81, 0X0E40,
    0X0A00, 0XCAC1, 0XCB81, 0X0B40, 0XC901, 0X09C0, 0X0880, 0XC841,
    0XD801, 0X18C0, 0X1980, 0XD941, 0X1B00, 0XDBC1, 0XDA81, 0X1A40,
    0X1E00, 0XDEC1, 0XDF81, 0X1F40, 0XDD01, 0X1DC0, 0X1C80, 0XDC41,
    0X1400, 0XD4C1, 0XD581, 0X1540, 0XD701, 0X17C0, 0X1680, 0XD641,
    0XD201, 0X12C0, 0X1380, 0XD341, 0X1100, 0XD1C1, 0XD081, 0X1040,
    0XF001, 0X30C0, 0X3180, 0XF141, 0X3300, 0XF3C1, 0XF281, 0X3240,
    0X3600, 0XF6C1, 0XF781, 0X3740, 0XF501, 0X35C0, 0X3480, 0XF441,
    0X3C00, 0XFCC1, 0XFD81, 0X3D40, 0XFF01, 0X3FC0, 0X3E80, 0XFE41,
    0XFA01, 0X3AC0, 0X3B80, 0XFB41, 0X3900, 0XF9C1, 0XF881, 0X3840,
    0X2800, 0XE8C1, 0XE981, 0X2940, 0XEB01, 0X2BC0, 0X2A80, 0XEA41,
    0XEE01, 0X2EC0, 0X2F80, 0XEF41, 0X2D00, 0XEDC1, 0XEC81, 0X2C40,
    0XE401, 0X24C0, 0X2580, 0XE541, 0X2700, 0XE7C1, 0XE681, 0X2640,
    0X2200, 0XE2C1, 0XE381, 0X2340, 0XE101, 0X21C0, 0X2080, 0XE041,
    0XA001, 0X60C0, 0X6180, 0XA141, 0X6300, 0XA3C1, 0XA281, 0X6240,
    0X6600, 0XA6C1, 0XA781, 0X6740, 0XA501, 0X65C0, 0X6480, 0XA441,
    0X6C00, 0XACC1, 0XAD81, 0X6D40, 0XAF01, 0X6FC0, 0X6E80, 0XAE41,
    0XAA01, 0X6AC0, 0X6B80, 0XAB41, 0X6900, 0XA9C1, 0XA881, 0X6840,
    0X7800, 0XB8C1, 0XB981, 0X7940, 0XBB01, 0X7BC0, 0X7A80, 0XBA41,
    0XBE01, 0X7EC0, 0X7F80, 0XBF41, 0X7D00, 0XBDC1, 0XBC81, 0X7C40,
    0XB401, 0X74C0, 0X7580, 0XB541, 0X7700, 0XB7C1, 0XB681, 0X7640,
    0X7200, 0XB2C1, 0XB381, 0X7340, 0XB101, 0X71C0, 0X7080, 0XB041,
    0X5000, 0X90C1, 0X9181, 0X5140, 0X9301, 0X53C0, 0X5280, 0X9241,
    0X9601, 0X56C0, 0X5780, 0X9741, 0X5500, 0X95C1, 0X9481, 0X5440,
    0X9C01, 0X5CC0, 0X5D80, 0X9D41, 0X5F00, 0X9FC1, 0X9E81, 0X5E40,
    0X5A00, 0X9AC1, 0X9B81, 0X5B40, 0X9901, 0X59C0, 0X5880, 0X9841,
    0X8801, 0X48C0, 0X4980, 0X8941, 0X4B00, 0X8BC1, 0X8A81, 0X4A40,
    0X4E00, 0X8EC1, 0X8F81, 0X4F40, 0X8D01, 0X4DC0, 0X4C80, 0X8C41,
    0X4400, 0X84C1, 0X8581, 0X4540, 0X8701, 0X47C0, 0X4680, 0X8641,
    0X8201, 0X42C0, 0X4380, 0X8341, 0X4100, 0X81C1, 0X8081, 0X4040,
  };

  uint8_t nTemp;
  uint16_t wCRCWord = 0xFFFF;

  while (wLength--) {
    nTemp = *nData++ ^ wCRCWord;
    wCRCWord >>= 8;
    wCRCWord ^= wCRCTable[nTemp];
  }
  return wCRCWord;
}

void printBuffer(uint8_t *buffer, size_t size)
{
  for (int i = 0; i < size; i++)
  {
    buffer[i]>15?Serial.print("0x"):Serial.print("0x0");
    Serial.print(buffer[i],16);
    Serial.print(" ");
  }
  Serial.println();
}

void setup() {
  // put your setup code here, to run once:
  Serial.begin(115200);
  Serial.println("Hello, ESP32!");

  CMD[0] = 0x01;
  CMD[1] = 0x65;
  CMD[2] = 0x00;
  CMD[3] = 0x00;
  CMD[4] = 0x00;
  CMD[5] = 0x02;
  CMD[6] = 0x04;
  CMD[7] = 0x00;
  CMD[8] = 0x01;
  CMD[9] = 0x00;
  CMD[10] = 0x0B;
  
  // CMD[11] = 0x01;
  // CMD[12] = 0x0C;

    Serial2.begin(SOFTSERIALBUADRATE, SERIAL_8N1, RXD2, TXD2);
    Serial2.setTimeout(50);
    Serial.println(F("RS485: Begin."));

}
int sizeResponse(uint8_t *buffer)
{
  // Serial.println(buffer[1]);
  switch (buffer[1])
  {
    case 0x64:
      return buffer[8];
      break;
    case 0x65:
      return buffer[3];
      break;
    case 0x66:
      return buffer[8];
      break;
    case 0x67:
      return buffer[10];
      break;
    case 0x68:
      return buffer[10];
      break;
    case 0x69:
      return buffer[5];
      break;
    case 0x70:
      return buffer[3];
      break;
    case 0x71:
      return buffer[14];
      break;
    case 0x84:
      return buffer[13];
      break;
    case 0x85:
      return buffer[20];
      break;
    default:

      break;
  }
  return 0;

}
byte *textTObyte(char *buffer)
{
    size_t bufferSize = strlen(buffer); // คำนวณขนาดของ buffer
    uint8_t *data = new uint8_t[bufferSize];
    for (int i = 0; i < bufferSize; i++)
    {
        data[i]=buffer[i];
    }
    return data;
}

void sendResponse(uint8_t id, uint8_t *buffer, size_t size)
{
    // ประกาศและสร้างอาเรย์ data ใหม่โดยมีขนาดของข้อมูล size+5 บายต์
    uint8_t *data = new uint8_t[size + 5];

    // กำหนดค่าเริ่มต้นในอาเรย์ data
    data[0] = 0x01; // ส่วนหัว
    data[1] = id;   // ไอดี
    data[2] = 0x02; // ความยาวข้อมูล
    if (id == 0x65 || id == 0x70)
    {
        data[3] = size + 5;
        // คัดลอกข้อมูลจาก buffer เข้าในอาเรย์ data ตั้งแต่ตำแหน่งที่ 3 เป็นต้นไป
        for (int i = 4; i < size + 4; i++)
        {
            data[i] = buffer[i - 4];
        }

        data[size + 4] = 0x03;     // ส่วนท้าย
    }
    else
    {

        // คัดลอกข้อมูลจาก buffer เข้าในอาเรย์ data ตั้งแต่ตำแหน่งที่ 3 เป็นต้นไป
        for (int i = 3; i < size + 3; i++)
        {
            data[i] = buffer[i - 3];
        }

        data[size + 3] = 0x03;     // ส่วนท้าย
        data[size + 4] = size + 5; // ความยาวทั้งหมด
    }

    // Serial.println(size);
    printBuffer(data, size + 5);
    uint16_t CRC = mosbusCRC16(data, size+5);
    Serial.println(CRC, 16);
    data[size+5] = highByte(CRC);
    data[size + 6] = lowByte(CRC);
    // Serial.println(highByte(CRC), 16);
    //   Serial.println(lowByte(CRC), 16);
    printBuffer(data, size + 7);

     Serial2.write(data, size + 7);
      // Serial2.write("ABCD", 4);
  //    WiFi.begin("Wokwi-GUEST", "", 6);
  // while (WiFi.status() != WL_CONNECTED) {
  //   delay(100);
  //   Serial.print(".");
  // }
}

// uint8_t wifiMAC[6];
void loop() {
  // put your main code here, to run repeatedly:
  // size_t sizeXX = sizeof(CMD);
  
// WiFi.macAddress(wifiMAC);
//  Serial.println(WiFi.macAddress().toString);
  //  Serial.println(WiFi.localIP());
  // IPAddress localIP = WiFi.localIP();
int x=0;
  //  uint8_t xx[] = {'H','e','l','l','o'};
    // size_t x = sizeof(xx) / sizeof(xx[0]);
    // char a[] = {"a","h","f"};
char xx[64] ;
    sprintf(xx,"%s","Hello world!");
sendResponse(0x65, textTObyte(xx), strlen(xx));

 delay(50); // this speeds up the simulation
uint8_t test[128];
uint8_t lenTEST;
  if (Serial2.available() > 0) {
    lenTEST = Serial2.readBytes(test, sizeof(test));
    Serial.println(lenTEST);
    printBuffer(test, lenTEST);
    uint16_t CRC = mosbusCRC16(test, lenTEST - 2);
        Serial.println(CRC, 16);
        //  data[size + 5] = highByte(CRC);
        //  data[size + 6] = lowByte(CRC);
        Serial.println(highByte(CRC), 16);
        Serial.println(lowByte(CRC), 16);
        if (highByte(CRC) != test[lenTEST-2] && lowByte(CRC) != test[lenTEST-1])
        {
            return;
        }
    // for(int i = 0; i < strlen(xx); i++) {
    //   Serial.print("0x");
    //   Serial.print(test[i], 16);
    //   Serial.print(" ");
    // }
    // Serial.println("");
  }

//  sendResponse(0x65, xx, x);
  // sendResponse(CMD);

  delay(2000); // this speeds up the simulation
}