#include <Arduino.h>
#include <SPI.h>
#include <Ethernet.h>
#include <ModbusMaster.h>
#include <WiFiClient.h>
#include <PubSubClient.h>
#include <mcp_can.h>
#include <CRC16.h>

// W5500 Ethernet Einstellungen
byte mac[] = { 0xDE, 0xAD, 0xBE, 0xEF, 0xFE, 0xED };
EthernetClient ethClient;
PubSubClient client(ethClient);

// RS485 Modbus Einstellungen
#define RS485_DE_RE 4
#define RS485_RX 16
#define RS485_TX 17
ModbusMaster node;

// RS232 Einstellungen
#define RS232_RX 27
#define RS232_TX 14

// CAN-Bus Einstellungen für MCP2515
#define CAN_CS 5
MCP_CAN CAN(CAN_CS);

void sendQuery();
void callback(char* topic, byte* payload, unsigned int length);
float hex2float(uint16_t high, uint16_t low);
void readCANBus(String &jsonPayload);
void sendToInverter(String &bmsData);

void setup() {
    Serial.begin(115200);
    Serial2.begin(2400, SERIAL_8N1, RS232_RX, RS232_TX);
    
    pinMode(RS485_DE_RE, OUTPUT);
    digitalWrite(RS485_DE_RE, LOW);
    Serial1.begin(19200, SERIAL_8N1, RS485_RX, RS485_TX);
    
    Serial.println("Initializing Ethernet...");
    if (Ethernet.begin(mac) == 0) {
        Serial.println("Failed to configure Ethernet using DHCP");
    }
    Serial.print("IP Address: ");
    Serial.println(Ethernet.localIP());
    
    client.setServer("192.168.1.1", 1883);
    client.setCallback(callback);
    
    node.begin(1, Serial1);

    // Initialisiere CAN-Bus
    if (CAN.begin(MCP_ANY, CAN_500KBPS, MCP_8MHZ) == CAN_OK) {
        Serial.println("CAN-Bus initialized successfully");
    } else {
        Serial.println("CAN-Bus initialization failed");
    }
    CAN.setMode(MCP_NORMAL);
}

void loop() {
    if (!client.connected()) {
        Serial.println("Connecting to MQTT...");
        if (client.connect("ESP32Client")) {
            Serial.println("Connected to MQTT");
        } else {
            delay(5000);
            return;
        }
    }
    client.loop();
    
    digitalWrite(RS485_DE_RE, HIGH);
    node.clearResponseBuffer();
    uint8_t result = node.readInputRegisters(0x0000, 76);
    digitalWrite(RS485_DE_RE, LOW);
    
    String jsonPayload = "{";
    
    if (result == node.ku8MBSuccess) {
        jsonPayload += "\"l1_voltage\":" + String(hex2float(node.getResponseBuffer(1), node.getResponseBuffer(0))) + ",";
        jsonPayload += "\"l2_voltage\":" + String(hex2float(node.getResponseBuffer(3), node.getResponseBuffer(2))) + ",";
        jsonPayload += "\"l3_voltage\":" + String(hex2float(node.getResponseBuffer(5), node.getResponseBuffer(4))) + ",";
        jsonPayload += "\"total_power\":" + String(hex2float(node.getResponseBuffer(53), node.getResponseBuffer(52))) + ",";
    }
    
    sendQuery(jsonPayload);
    readCANBus(jsonPayload);
    client.publish("modbus_rs232/data", jsonPayload.c_str());
    
    sendToInverter(jsonPayload);
    
    delay(1000);
}

void readCANBus(String &jsonPayload) {
    long unsigned int rxId;
    unsigned char len = 0;
    unsigned char rxBuf[8];
    
    if (CAN.checkReceive() == CAN_MSGAVAIL) {
        CAN.readMsgBuf(&rxId, &len, rxBuf);
        
        jsonPayload += "\"bms_voltage\":" + String((rxBuf[0] << 8 | rxBuf[1]) / 10.0) + ",";
        jsonPayload += "\"bms_current\":" + String((rxBuf[2] << 8 | rxBuf[3]) / 10.0) + ",";
        jsonPayload += "\"bms_soc\":" + String(rxBuf[4]) + ",";
    }
}

void sendToInverter(String &bmsData) {
    String inverterCommand = "^D054BMS";
    
    long unsigned int rxId;
    unsigned char len = 0;
    unsigned char rxBuf[8];
    
    if (CAN.checkReceive() == CAN_MSGAVAIL) {
        CAN.readMsgBuf(&rxId, &len, rxBuf);
        
        inverterCommand += String(rxBuf[0] << 8 | rxBuf[1]); // BMS Voltage
        inverterCommand += "," + String(rxBuf[4]); // Battery percent
        inverterCommand += ",1,0011,0,0,"; // Fixed values
        inverterCommand += String(rxBuf[2] << 8 | rxBuf[3]); // Current
        inverterCommand += ",0576,0576,0950,0,0,0448,0560";
    }
    
    CRC16 crc;
    crc.reset();
    crc.add((uint8_t*)inverterCommand.c_str(), inverterCommand.length());
    uint16_t crcValue = crc.getCRC();
    
    inverterCommand += " + " + String(crcValue, HEX) + "\r";
    
    Serial2.println(inverterCommand);
    Serial.println("Sent to inverter: " + inverterCommand);
}