const int MC9_CH[] = {1000, 1008, 1016, 1024, 1100, 1108, 1116, 1124};

// 10DWR,02,0302,0001,0501,0001
// Auto Tune. 0302(Ch Sel) 0001(Ch.1) 0501(AT) 0001(On, Off=0000)

void setupMC9_1(int i, int data){
  RS485_1_Addr[i] = data;

  Serial.println("MC9 No."+ String(i) + " 's address set as " + String(data));
}

String msg_MC9_PV(int addr){
  String message = "";

  if(addr < 10){
    message += "0";  
  }
  message += String(addr);    // Address
  message += "DRS,08,0001";   // Data Read Seris 08 data from 0001(PV) ch
  message += sumMC9(message); // Check Sum
  message = "" + message;    // Start with STX

  return message;
}

String msg_MC9_SV(int addr){
  String message = "";
  
  if(addr < 10){
    message += "0";  
  }
  message += String(addr);    // Address
  message += "DRS,08,0011";   // Data Read Seris 08 data from 0001(SV) ch
  message += sumMC9(message); // Check Sum
  message = "" + message;    // Start with STX

  return message;
}

int msg_MC9_SV_set(unsigned int data[], int dataSize){
  String message = "";
  int lp0, lp1;

  if(dataSize > 8){
    lp0 = 8;
    lp1 = dataSize;
  }else{
    lp0 = dataSize;
    lp1 = 0;
  }

  if(RS485_1_Addr[0] != 0){
    if(RS485_1_Addr[0] < 10){
      message += "0";  
    }
    message = String(RS485_1_Addr[0]);
    message += "DWR,0" + String(lp0) + ",";
    for(int i=0 ; i<lp0 ; i++){
      char hexStr[5]; // Buffer to hold the hexadecimal string (4 characters + null terminator)
      sprintf(hexStr, "%04X", data[i]); // Format as 4 digit uppercase hexadecimal

      message += MC9_CH[i] + 1; // ZONE 1
      message += ",";
      message += String(hexStr);
      if(i != (lp0-1)){
        message += ",";
      }
    }
    message += sumMC9(message);
    message = "" + message + CRLF;
    write_buff_first(Buff_485_1_Wr, message);
  }

  if((RS485_1_Addr[1] != 0) && (lp1>0)){
    if(RS485_1_Addr[1] < 10){
      message += "0";  
    }
    message = String(RS485_1_Addr[1]);
    message += "DWR,0" + String(lp1-8) + ",";
    for(int i=8 ; i<lp1 ; i++){
      char hexStr[5]; // Buffer to hold the hexadecimal string (4 characters + null terminator)
      sprintf(hexStr, "%04X", data[i]); // Format as 4 digit uppercase hexadecimal

      message += MC9_CH[i%8] + 1; // ZONE 1
      message += ",";
      message += String(hexStr);
      if(i != (lp1-1)){
        message += ",";
      }
    }
    message += sumMC9(message);
    message = "" + message + CRLF;
    write_buff_first(Buff_485_1_Wr, message);
  }

  return 0;
}

int msg_MC9_AT_set(unsigned int data[], int dataSize){
  String message = "";
  bool at[16];

  int lp0, lp1;
  
  // Number to boolean array
  unsigned int data0 = data[0];
  for (int i = 0; i < 16; i++) {
    at[i] = (bitRead(data0, i) ? HIGH : LOW);
  }

  lp0 = 8;
  lp1 = 8;

  message = "";
  if(RS485_1_Addr[0] != 0){
    if(RS485_1_Addr[0] < 10){
      message += "0";  
    }
    message += String(RS485_1_Addr[0]);
    message += "DWR,16,";
    for(int i=0 ; i<8 ; i++){
      String atFlag;
      if(at[i]){
        atFlag = "0001";  // Auto Tuen On
      }else{
        atFlag = "0000";  // Auto Tuen Off
      }

      message += "0302,"; // Ch No cmd.
      message += "000" + String(i+1) + ","; // Ch No.
      message += "0501,"; // AT cmd.
      message += atFlag;  // AT flag
      if(i != 7){
        message += ",";
      }
    }
    message += sumMC9(message);
    message = "" + message + CRLF;
    write_buff_first(Buff_485_1_Wr, message);
  }

  message = "";
  if((RS485_1_Addr[1] != 0) && (lp1>0)){
    if(RS485_1_Addr[1] < 10){
      message += "0";  
    }
    message += String(RS485_1_Addr[1]);
    message += "DWR,16,";
    for(int i=0 ; i<8 ; i++){
      String atFlag;
      if(at[i+8]){
        atFlag = "0001";
      }else{
        atFlag = "0000";
      }

      message += "0302,"; // Ch No cmd.
      message += "000" + String(i+1) + ","; // Ch No.
      message += "0501,"; // AT cmd.
      message += atFlag;  // AT flag
      if(i != 7){
        message += ",";
      }
    }
    message += sumMC9(message);
    message = "" + message + CRLF;
    write_buff_first(Buff_485_1_Wr, message);
  }

  return 0;
}

int saveMC9(String message){
  int addr;
  int idx = -1;
  String mode;
  int data[8];
  int crc;

  if (!parseMC9(message, addr, mode, data, crc)) {
    for(int i=0 ; i < 2 ; i++){ // RS485_1 has two rooms for two MC9s
      if(RS485_1_Addr[i] == addr){
        idx = i;
        break;
      }
    }
    
    if(idx < 0){
      return -1;
    }

    if(latest_sent_msg.indexOf("DRS,08,0001") != -1){ // if sent message is PV CMD
      //rcv_10_PV = true;
      RS485_1_Rcv_PV[idx] = true;
      for(int i = 0 ; i < RS485_1_Rcv_size ; i++){
        RS485_1_Values_PV[i + idx*8] = data[i];
      }
    }
    
    if(latest_sent_msg.indexOf("DRS,08,0011") != -1){ // if sent message is SV CMD
      //rcv_10_SV = true;
      RS485_1_Rcv_SV[idx] = true;
      for(int i = 0 ; i < RS485_1_Rcv_size ; i++){
        RS485_1_Values_SV[i + idx*8] = data[i];
      }
    }
    return 0;

  } else {
      Serial.println("error 485 read");
      return -1;
  }
}

int timeoutMC9(){
  int addr;
  int idx = -1;
  String msg, mode, cmd;
  char dummy[4];
  int data[8];
  int crc;
  // Message parsing
  
  msg = latest_sent_msg.substring(1);
  sscanf(msg.c_str(), "%2d%3s", &addr, &dummy);

  Serial.print("485 not responced... (Timeout) Req msg : " + latest_sent_msg);

  if (latest_sent_msg.indexOf("DRS") != -1) {
    for(int i=0 ; i < 2 ; i++){ // RS485_1 has two rooms for two MC9s
      // Find idx of array
      if(RS485_1_Addr[i] == addr){
        idx = i;
        break;
      }
    }

    // If not matched, return error
    if(idx < 0){
      Serial.println();
      return -1;
    }

    if(latest_sent_msg.indexOf("DRS,08,0001") != -1){ // if sent message is PV CMD
      RS485_1_Rcv_PV[idx] = false;
      Serial.print(" >> PV @");
      Serial.println(idx);
    }
    if(latest_sent_msg.indexOf("DRS,08,0011") != -1){ // if sent message is SV CMD
      RS485_1_Rcv_SV[idx] = false;
      
      Serial.print(" >> SV @");
      Serial.println(idx);
    }
    return 0;

  } else {
      Serial.println();
      return -1;
  }

  /*
  if(latest_sent_msg == MC9_10_PV){
    //rcv_10_PV = false;
  }
  if(latest_sent_msg == MC9_10_SV){
    //rcv_10_SV = false;

  }else {
    Serial.println("error 485 read");
    return -1;
  }
  */
}

int parseMC9(const String& message, int& addr, String& mode, int data[8], int& crc) {
    char addrC[3], modeC[4], statusC[3];
    int dataC[8], crcC;

    // Remove start-of-text character if present
    if (message[0] == '\x02') {
      message = message.substring(1);
    }

    // Parse the MC9 message
    int ret = sscanf(message.c_str(), "%2s%3s,%2s,%4x,%4x,%4x,%4x,%4x,%4x,%4x,%4x%2x", 
        addrC, modeC, statusC, &dataC[0], &dataC[1], &dataC[2], &dataC[3], &dataC[4], &dataC[5], &dataC[6], &dataC[7], &crcC);

    // Check the parsed status and the number of extracted elements
    if (strcmp(statusC, "OK") != 0 || ret != 12) {
        return -1;
    }
    
    // Copy data to the output parameters
    addr = atoi(addrC);
    mode = String(modeC);
    for (int i = 0; i < 8; i++) {
        data[i] = dataC[i];
    }
    crc = crcC;

    return 0;
}


String sumMC9(String input) {
    int sum = 0;

    for (char c : input) {
        sum += c;
    }
    sum = sum & 0xFF;  // Make sure we only keep the least significant byte
    String hexSum = String(sum, HEX);  // Convert the sum to hexadecimal
    hexSum.toUpperCase();  // Make it upper-case

    return hexSum;
}