#include <Wire.h>
#include <RTClib.h>

#include <NeoSWSerial.h>
#include <ModbusMaster.h>
#include "util.h"
#include "register_map_vsd.h"

// #include <SD.h>

#include <SPI.h>
#include <SdFat.h>
#define SD_CS_PIN 10 // Chip Select for SD Card
// RS485 pins
#define DE_RE_PIN 4
#define RX_PIN 8 // SoftwareSerial RX pin
#define TX_PIN 7 // SoftwareSerial TX pin
#define SLAVE_ID 1 //From 1 – 10 (Normally 1)
#define SERIAL_BAUDRATE 9600
#define MODBUS_SERIAL_BAUDRATE 19200
#define SERIAL_CONFIG SERIAL_8E1 
#define LED_A_PID 3
#define LED_B_PID 5

// Try to select the best SD card configuration.
#define SPI_CLOCK SD_SCK_MHZ(50)
#if HAS_SDIO_CLASS
#define SD_CONFIG SdioConfig(FIFO_SDIO)
#elif ENABLE_DEDICATED_SPI
#define SD_CONFIG SdSpiConfig(SD_CS_PIN, DEDICATED_SPI, SPI_CLOCK)
#else // HAS_SDIO_CLASS
#define SD_CONFIG SdSpiConfig(SD_CS_PIN, SHARED_SPI, SPI_CLOCK)
#endif // HAS_SDIO_CLASS

RTC_DS3231 rtc; // Create an RTC object
SdFat32 sd;
// SdExFat sd;
File dataFile;
NeoSWSerial modbusSerial(RX_PIN, TX_PIN); // Create a software serial instance
ModbusMaster node;

unsigned long lastRefreshTime = 0;
bool headerWritten = 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"));

  // Initialize RTC
  if (!rtc.begin())
  {
    Serial.println(F("Couldn't find RTC\n"));
    flicker(LED_B_PID, 4, 1000); // 4 times on LED b is RTC Error
    digitalWrite(LED_B_PID, HIGH);
    digitalWrite(LED_A_PID, HIGH);
    return;
  }

  if (rtc.lostPower())
  {
    Serial.println(F("RTC lost power, let's set the time!\n"));
    // Comment out the following line once the time is set to avoid resetting on every start
    rtc.adjust(DateTime(F(__DATE__), F(__TIME__)));
    flicker(LED_B_PID, 4, 500); // 6 times fast on LED b is RTC reset
  }

  Serial.print(F("Time: "));
  Serial.print(rtc.now().timestamp());
  Serial.println(F("\n"));

  // Initialize SD card
  Serial.println(F("SD card initializing..."));
  pinMode(SD_CS_PIN, OUTPUT);

  // if (!SD.begin(SPI_HALF_SPEED, SD_CS_PIN ))
  // {
  //     Serial.println(F("SD card initialization failed!\n"));
  //     return;
  // }
  // Initialize the SD.
  if (!sd.begin(SD_CONFIG))
  {
    flicker(LED_B_PID, 2, 1000);
    digitalWrite(LED_B_PID, HIGH);
    sd.initErrorHalt(&Serial);

    // 2 Times slow and stay on, SD Card initilize error
    return;
  }
  Serial.println(F("SD card initialized.\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(MODBUS_SERIAL_BAUDRATE);

  node.begin(SLAVE_ID, modbusSerial);
  node.preTransmission(preTransmission);
  node.postTransmission(postTransmission);
  flicker(LED_B_PID, 10, 100);
  digitalWrite(LED_B_PID, LOW);
  booted = true;
}

void preTransmission()
{
  // Serial.println(F("Transmitting Start"));
  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);
  // Serial.println(F("Transmitting End"));
}

void primeFileDate()
{
  if (!dataFile)
  {
    Serial.println(F("Error opening file"));
    return;
  }
  DateTime now = rtc.now();
  // Log the current date and time
  dataFile.print("\n");
  dataFile.print(now.year(), DEC);
  dataFile.print('-');
  dataFile.print(now.month(), DEC);
  dataFile.print('-');
  dataFile.print(now.day(), DEC);
  dataFile.print(" ");
  dataFile.print(now.hour(), DEC);
  dataFile.print(':');
  dataFile.print(now.minute(), DEC);
  dataFile.print(':');
  dataFile.print(now.second(), DEC);
  dataFile.print(F(","));
}

String getFilename()
{
  DateTime now = rtc.now();
  String mb = F("pm8k_");
  mb += now.year();
  mb += now.month();
  mb += now.day();
  mb += F(".csv");

  return mb;
}

// const char[20] filename = "20240523.csv";

void loop()
{
  if (!booted)
  {
    Serial.print(F("\nBoot failed, cycle "));

    delay(10000);
    digitalWrite(LED_A_PID, LOW);
    return;
  }
  delay(100);
  String writebuffer;

  if (millis() - lastRefreshTime >= 1000)
  {
    lastRefreshTime += 1000;

    Serial.print(F("\nTime: "));
    Serial.print(rtc.now().timestamp());
    //   Serial.print("\nHeep:");
    //  Serial.print(ESP.getFreeHeap());
    const uint16_t totalReg = sizeof(registers) / sizeof(registers[0]);
    float rawValues[totalReg] = {0};
    // Open File
    String filename = getFilename();
    Serial.print(F("Open Card "));
    Serial.print(filename.c_str());
    Serial.print("\n");
    if (!dataFile.open(filename.c_str(), FILE_WRITE))
    {
      flicker(LED_B_PID, 6, 500); // Six quick flickers. SD Card error
      Serial.println(F("Failed to Open Card "));
    }
    if (!headerWritten)
    {
      dataFile.print("\nDate Time,");
      for (int i = 0; i < totalReg; i++)
      {
        const uint16_t regaddr = pgm_read_word(&registers[i].regaddr);
        dataFile.print("@");
        dataFile.print(regaddr);
        dataFile.print(",");
      }
      headerWritten = true;
      flicker(LED_A_PID, 50, 10); // 10  flickers, written header
    }
    primeFileDate();

    Serial.print("\n");
    Serial.println(totalReg);
  
    // First pass - read all base registers
    
    
    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);
      
      if (regaddr > 0 && regtype <= 3) { // Only read direct registers
        delay(10);
        uint8_t result = node.readHoldingRegisters(regaddr - 1, 2);
        delay(10);

        if (result == node.ku8MBSuccess) {
          switch(regtype) {
            case 1: 
              rawValues[i] = node.getResponseBuffer(0);
              break;
            case 2:
              rawValues[i] = getRegisterFloat(node.getResponseBuffer(0), node.getResponseBuffer(1));
              break;
            case 3:
              rawValues[i] = getRegisterInt64(node.getResponseBuffer(0), node.getResponseBuffer(1), 
                                            node.getResponseBuffer(2), node.getResponseBuffer(3));
              break;
          }
        }
      }
    }

    // Second pass - process all registers including derived values
    for (int i = 0; i < totalReg; i++) {
      const uint8_t regtype = pgm_read_word(&registers[i].regtype);
      const float scale = pgm_read_float(&registers[i].scale);
      float value = 0;

      switch(regtype) {
        case 1:
        case 2:
        case 3:
          value = rawValues[i] * scale;
          break;
        case 4:
          value = calculateStatusWord(rawValues) * scale;
          break;
        case 5:
          value = calculateThermal(rawValues) * scale;
          break;
        case 6:
          value = calculatePower(rawValues) * scale;
          break;
        case 7:
          value = calculateRPM(rawValues) * scale;
          break;
      }

      dataFile.print(value);
      dataFile.print(",");
    }
    

    Serial.print(F("\nRead buffer: "));
    delay(10);

    if (dataFile)
    {
      dataFile.close(); // Close the file
      Serial.print(F("Data written to SD card: "));
      Serial.print(filename.c_str());
      Serial.print(F("\n"));
    }

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

    flicker(LED_A_PID, 4, 100); // Cycle written 4 quick flickers
  }

  // // Check if the read was successful
}