#include <NeoSWSerial.h>
#include <ModbusMaster.h>
#include <SoftwareSerial.h>
#include "util.h"
#include "register_map_pm8000.h"

// RS485 pins
#define DE_RE_PIN 4
#define RX_PIN 8 // SoftwareSerial RX pin for Modbus
#define TX_PIN 7 // SoftwareSerial TX pin for Modbus
#define SLAVE_ID 101
#define SERIAL_BAUDRATE 9600
#define LED_A_PID 3
#define LED_B_PID 5

// GSM module pins
#define GSM_RX 2
#define GSM_TX 3

NeoSWSerial modbusSerial(RX_PIN, TX_PIN); // Create a software serial instance for Modbus
ModbusMaster node;
SoftwareSerial gsmSerial(GSM_RX, GSM_TX); // Create a software serial instance for GSM

unsigned long lastRefreshTime = 0;
bool headerSent = false;
bool booted = false;

void flicker(uint8_t pin, uint8_t times, uint16_t speed)
{
  for (int i = 0; i < times; i++)
  {
    delay(speed);
    digitalWrite(pin, HIGH);
    delay(speed);
    digitalWrite(pin, LOW);
  }
}

void setup()
{
  booted = false;
  pinMode(LED_A_PID, OUTPUT);
  pinMode(LED_B_PID, OUTPUT);
  digitalWrite(LED_A_PID, LOW);
  digitalWrite(LED_B_PID, HIGH);

  Serial.begin(SERIAL_BAUDRATE); // For debugging
  Serial.println(F("Startup \n"));

  Serial.println(F("Initialize RS485 module / Modbus \n"));

  pinMode(DE_RE_PIN, OUTPUT);
  digitalWrite(DE_RE_PIN, LOW); // Set to LOW for receiving mode initially
  modbusSerial.begin(SERIAL_BAUDRATE);

  node.begin(SLAVE_ID, modbusSerial);
  node.preTransmission(preTransmission);
  node.postTransmission(postTransmission);

  // Initialize GSM module
  gsmSerial.begin(9600);
  delay(3000); // Give time for GSM module to initialize
  
  if (initGSM()) {
    Serial.println(F("GSM module initialized successfully"));
    flicker(LED_A_PID, 5, 200); // 5 quick flashes to indicate successful GSM initialization
  } else {
    Serial.println(F("Failed to initialize GSM module"));
    flicker(LED_B_PID, 2, 1000); // 2 slow flashes to indicate GSM initialization failure
    return;
  }

  flicker(LED_A_PID, 10, 100);
  digitalWrite(LED_B_PID, LOW);
  booted = true;
}

void preTransmission()
{
  digitalWrite(DE_RE_PIN, HIGH); // Enable RS485 transmit
  digitalWrite(LED_A_PID, HIGH);
}

void postTransmission()
{
  digitalWrite(DE_RE_PIN, LOW); // Disable RS485 transmit
  digitalWrite(LED_A_PID, LOW);
}

bool initGSM() {
  gsmSerial.println("AT");
  delay(1000);
  if (gsmSerial.find("OK")) {
    gsmSerial.println("AT+CPIN?");
    delay(1000);
    if (gsmSerial.find("READY")) {
      return true;
    }
  }
  return false;
}

bool waitForResponse(const char* expectedResponse, unsigned long timeout) {
  unsigned long start = millis();
  String response = "";
  while (millis() - start < timeout) {
    if (gsmSerial.available()) {
      char c = gsmSerial.read();
      response += c;
      if (response.indexOf(expectedResponse) != -1) {
        return true;
      }
    }
  }
  Serial.println("Timeout waiting for response: " + response);
  return false;
}

int16_t waitForHTTPAction(unsigned long timeout) {
  unsigned long start = millis();
  String response = "";
  while (millis() - start < timeout) {
    if (gsmSerial.available()) {
      char c = gsmSerial.read();
      response += c;
      if (response.indexOf("+HTTPACTION:") != -1) {
        // Parse the response
        int status = response.substring(response.indexOf(',') + 1).toInt();
        return status;
      }
    }
  }
  Serial.println("Timeout waiting for HTTP action: " + response);
  return -1;
}

bool sendGSMData(String data) {
  // Configure bearer profile
  gsmSerial.println("AT+SAPBR=3,1,\"Contype\",\"GPRS\"");
  if (!waitForResponse("OK", 1000)) return false;
  
  gsmSerial.println("AT+SAPBR=3,1,\"APN\",\"afrihost\""); // Replace with your APN
  if (!waitForResponse("OK", 1000)) return false;
  
  gsmSerial.println("AT+SAPBR=1,1");
  if (!waitForResponse("OK", 10000)) return false; // Bearer activation can take longer

  // Initialize HTTP service
  gsmSerial.println("AT+HTTPINIT");
  if (!waitForResponse("OK", 1000)) return false;
  
  gsmSerial.println("AT+HTTPPARA=\"CID\",1");
  if (!waitForResponse("OK", 1000)) return false;

  // Set the URL (replace with your server URL)
  gsmSerial.println("AT+HTTPPARA=\"URL\",\"http://hardwareapi.warky.dev/upload/pm8000\"");
  if (!waitForResponse("OK", 1000)) return false;

  // Set HTTP data
  gsmSerial.println("AT+HTTPPARA=\"CONTENT\",\"application/x-www-form-urlencoded\"");
  if (!waitForResponse("OK", 1000)) return false;
  
  gsmSerial.println("AT+HTTPDATA=" + String(data.length()) + ",5000");
  if (!waitForResponse("DOWNLOAD", 1000)) return false;
  
  gsmSerial.println(data);
  if (!waitForResponse("OK", 5000)) return false;

  // Send HTTP POST request
  gsmSerial.println("AT+HTTPACTION=1");
  int16_t httpStatus = waitForHTTPAction(10000);
  if (httpStatus != 200) {
    Serial.println("HTTP POST failed with status: " + String(httpStatus));
    return false;
  }

  // Read the response
  gsmSerial.println("AT+HTTPREAD");
  if (!waitForResponse("OK", 1000)) return false;

  // Close HTTP service
  gsmSerial.println("AT+HTTPTERM");
  if (!waitForResponse("OK", 1000)) return false;

  // Close bearer
  gsmSerial.println("AT+SAPBR=0,1");
  if (!waitForResponse("OK", 1000)) return false;

  digitalWrite(LED_A_PID, HIGH); // Indicate successful data send
  delay(200);
  digitalWrite(LED_A_PID, LOW);

  return true;
}

void loop()
{
  if (!booted)
  {
    Serial.print(F("\nBoot failed, cycle "));
    delay(10000);
    digitalWrite(LED_A_PID, LOW);
    return;
  }
  
  delay(100);
  String data;

  if (millis() - lastRefreshTime >= 60000) // Changed to 1 minute interval
  {
    lastRefreshTime = millis();

    Serial.print(F("\nTime: "));
    Serial.print(millis());

    const uint16_t totalReg = sizeof(registers) / sizeof(registers[0]);

    if (!headerSent)
    {
      data = "Date Time,";
      for (int i = 0; i < totalReg; i++)
      {
        const uint16_t regaddr = pgm_read_word(&registers[i].regaddr);
        data += "@";
        data += String(regaddr);
        data += ",";
      }
      headerSent = true;
      if (sendGSMData(data)) {
        flicker(LED_A_PID, 50, 10); // 50 quick flickers, sent header
      } else {
        flicker(LED_B_PID, 5, 200); // 5 medium flashes, failed to send header
      }
    }

    data = String(millis()) + ","; // Use millis() as timestamp

    Serial.println(totalReg);
    // Modbus Data Loop
    for (int i = 0; i < totalReg; i++)
    {
      const uint16_t regaddr = pgm_read_word(&registers[i].regaddr);
      const uint8_t regtype = pgm_read_word(&registers[i].regtype);

      Serial.print(F("Reg Read: "));
      Serial.println(regtype);
      Serial.println(regaddr);

      if (regaddr > 0)
      {
        delay(25); // Gives the pending communication a little delay
        uint8_t result = node.readHoldingRegisters(regaddr - 1, 2);
        delay(25); // Delay the read for a little bit so that the buffer can be read

        if (result == node.ku8MBSuccess)
        {
          if (regtype == 2)
          {
            data += String(getRegisterFloat(node.getResponseBuffer(0), node.getResponseBuffer(1)));
          }
          else if (regtype == 1)
          {
            data += String(node.getResponseBuffer(0));
          }
          else if (regtype == 0)
          {
            data += String(getRegisterInt32(node.getResponseBuffer(0), node.getResponseBuffer(1)));
          }
          else if (regtype == 5)
          {
            String strData = "";
            for (uint8_t j = 0; j < 20; j++)
            {
              uint8_t v = node.getResponseBuffer(j);
              if (v == 0) {
                break;
              }
              strData += char(v);
            }
            data += strData;
          }
          else
          {
            data += "null";
          }
        }
        else
        {
          Serial.print(F("Reg Error: "));
          Serial.print(result, HEX);
          Serial.print("\n");
          data += "E";
          data += String(result, HEX);
          flicker(LED_B_PID, 2, 250);
        }
        data += ",";
      }
    }

    Serial.print(F("\nData to send: "));
    Serial.println(data);

    if (sendGSMData(data)) {
      Serial.println(F("Data sent successfully via GSM"));
      flicker(LED_A_PID, 4, 100); // 4 quick flickers on LED_A_PID, data sent
    } else {
      Serial.println(F("Failed to send data via GSM"));
      flicker(LED_B_PID, 4, 250); // 4 medium flashes on LED_B_PID, failed to send data
    }

    Serial.print(F("\n\n"));
  }
}