#include "BLEDevice.h"
#include <ETH.h>
#include <NTPClient.h>
#include <WiFiUdp.h>
#include <WiFiClientSecure.h>
#include <HTTPClient.h>
#include "ca_cert.h"
#include "config.h"

// NTP Client setup
WiFiUDP ntpUDP;
NTPClient timeClient(ntpUDP, "pool.ntp.org");

static BLEUUID serviceUUID("0000ff00-0000-1000-8000-00805f9b34fb"); //xiaoxiang bms service
static BLEUUID charUUID_rx("0000ff01-0000-1000-8000-00805f9b34fb"); //xiaoxiang bms rx id
static BLEUUID charUUID_tx("0000ff02-0000-1000-8000-00805f9b34fb"); //xiaoxiang bms tx id

typedef struct
{
    byte start;
    byte type;
    byte status;
    byte dataLen;
} bmsPacketHeaderStruct;

char currentName[128];
bool gotBasicInfo;
bool gotCellInfo;
static bool eth_ready = false;

WiFiClientSecure client;
HTTPClient https;

void setup() {
    Serial.begin(115200);
    eth_begin();
    timeClient.begin();
    client.setCACert(ca_cert);
    client.setHandshakeTimeout(5);
    esp_bt_controller_mem_release(ESP_BT_MODE_CLASSIC_BT);
    BLEDevice::init(""); // Initialize BLE device
}

bool client_connect(){
    //https.setTimeout(preferences.getInt("http_timeout"));
    //https.setConnectTimeout(preferences.getInt("http_timeout"));
    //https.addHeader("Content-Type","application/x-www-form-urlencoded");
    String url="URL";
    https.end();
    if(!https.begin(client, url)) {  // HTTPS
        Serial.println("connect fail");
        return false;
    }
    Serial.println("connect ok");
    return true;
}

void loop() {
    Serial.printf("\r\n\r\n===============================\r\n\r\n");

    Serial.println("Scanning...");

    BLEScan* pBLEScan = BLEDevice::getScan(); // Create new scan
    pBLEScan->setActiveScan(true); // Active scan uses more power, but get results faster
    BLEScanResults foundDevices = pBLEScan->start(5); //seconds

    Serial.println("Devices found: " + String(foundDevices.getCount()));

    while(!eth_ready){
        Serial.println("Wait for eth...");
        delay(250);
    }
    timeClient.update();
    //testClient("www.google.com", 80);
    client_connect();

    for (int i = 0; i < foundDevices.getCount(); i++) {
        delay(1000);
        Serial.printf("\r\n\r\n===============================\r\n\r\n");

        BLEAdvertisedDevice advertisedDevice = foundDevices.getDevice(i);
        Serial.println("\nFound Device: " + String(advertisedDevice.toString().c_str()));

        client_connect();

        std::string targetAddress = "d0:65:de:e5:89:76";
        if (advertisedDevice.getAddress().toString() == targetAddress) {
            Serial.println("Victron device found!");
            decodeVictron(advertisedDevice);
            break;
        }

        if(!advertisedDevice.isAdvertisingService(serviceUUID)) {
            Serial.println("Device does not advertise the specified service UUID.");
            continue;
        }

        BLEClient* pClient = BLEDevice::createClient();
        Serial.println("Connecting to: " + String(advertisedDevice.getName().c_str()));

        int retryCount = 0;
        const int maxRetries = 5; // Maximum number of retries
        while(retryCount < maxRetries) {
            if(pClient->connect(&advertisedDevice)) {
                Serial.println("Connected successfully.");
                break; // Exit the loop if the connection is successful
            } else {
                Serial.println("Failed to connect. Retrying...");
                retryCount++; // Increment the retry counter
                delay(1000); // Optional: Wait for a second before retrying
            }
        }
        if(retryCount == maxRetries) {
            Serial.println("Failed to connect after retries.");
            continue;
        }

        // Get remote service
        Serial.println("Get remote service...");
        BLERemoteService* pRemoteService = pClient->getService(serviceUUID);
        if (pRemoteService == nullptr){
            Serial.println(String("BLE: failed to find service UUID"));
            Serial.print("Failed to find our service UUID: ");
            Serial.println(serviceUUID.toString().c_str());
            pClient->disconnect();
            continue;
        }

        // Get BMS receive characteristic
        Serial.println("Get BMS receive characteristic...");
        BLERemoteCharacteristic* pRemoteCharacteristic_rx = pRemoteService->getCharacteristic(charUUID_rx);
        if (pRemoteCharacteristic_rx == nullptr){
            Serial.println(String("BLE: failed to find RX UUID"));
            Serial.print("Failed to find rx UUID: ");
            Serial.println(charUUID_rx.toString().c_str());
            pClient->disconnect();
            continue;
        }

        // Register callback for remote characteristic (receive channel)
        Serial.println("Register callback for remote characteristic...");
        if (pRemoteCharacteristic_rx->canNotify()){
            pRemoteCharacteristic_rx->registerForNotify(MyNotifyCallback);
        }else{
            Serial.println(String("BLE: failed to register notification of remote characteristic"));
            Serial.println("Failed to register notification of remote characteristic");
            pClient->disconnect();
            continue;
        }

        // Get BMS transmit characteristic
        Serial.println("Get BMS transmit characteristic...");
        BLERemoteCharacteristic* pRemoteCharacteristic_tx = pRemoteService->getCharacteristic(charUUID_tx);
        if (pRemoteCharacteristic_tx == nullptr){
            Serial.println(String("BLE: failed to find TX UUID"));
            Serial.print("Failed to find tx UUID: ");
            Serial.println(charUUID_tx.toString().c_str());
            pClient->disconnect();
            continue;
        }

        // Check whether tx is writeable
        Serial.println("Check whether tx is writeable...");
        if (!(pRemoteCharacteristic_tx->canWriteNoResponse())){
            Serial.println(String("BLE: failed TX remote characteristic is not writable"));
            Serial.println("Failed TX remote characteristic is not writable");
            pClient->disconnect();
            continue;
        }

        Serial.println("Get data...");

        gotBasicInfo=false;
        gotCellInfo=false;

        std::__cxx11::string deviceName = advertisedDevice.getName();
        strncpy(currentName, deviceName.c_str(), sizeof(currentName) - 1);
        currentName[sizeof(currentName) - 1] = '\0'; // Ensure null-termination
        // Convert to lowercase and replace spaces with dots
        for (char* p = currentName; *p; ++p) {
            *p = *p == ' ' ? '.' : *p;
        }

        unsigned long start=millis();
        while( millis()-start<10000 && (gotBasicInfo==false || gotCellInfo==false) ){
            // REQUEST BASIC INFO
            if(gotBasicInfo==false){
                Serial.println("Request Basic Info");
                delay(500);
                // header status command length data checksum footer
                //   DD     A5      03     00    FF     FD      77
                uint8_t a_data[7] = {0xdd, 0xa5, 3, 0x0, 0xff, 0xfd, 0x77};
                pRemoteCharacteristic_tx->writeValue(a_data, sizeof(a_data), false);
            }

            // REQUEST CELL INFO
            if(gotCellInfo==false){
                Serial.println("Request Cell Info");
                delay(500);
                // header status command length data checksum footer
                //   DD     A5      03     00    FF     FD      77
                uint8_t b_data[7] = {0xdd, 0xa5, 4, 0x0, 0xff, 0xfc, 0x77};
                pRemoteCharacteristic_tx->writeValue(b_data, sizeof(b_data), false);
            }
        }
        pClient->disconnect();
    }
    Serial.println("Reboot!");
    delay(100);
    ESP.restart();
}

static void MyNotifyCallback(BLERemoteCharacteristic *pBLERemoteCharacteristic, uint8_t *pData, size_t length, bool isNotify){
  //hexDump((char*)pData, length);
  if(!bleCollectPacket((char *)pData, length)){
    Serial.println("ERROR: packet could not be collected.");
    }
}

void hexDump(const char *data, uint32_t dataSize)
{
    Serial.println("HEX data:");

    for (int i = 0; i < dataSize; i++)
    {
        Serial.printf("0x%x, ", data[i]);
    }
    Serial.println("");
}

bool bleCollectPacket(char *data, uint32_t dataSize) // reconstruct packet, called by notifyCallback function
{
    static uint8_t packetstate = 0; //0 - empty, 1 - first half of packet received, 2- second half of packet received

    // packet sizes:
    //  (packet ID 03) = 4 (header) + 23 + 2*N_NTCs + 2 (checksum) + 1 (stop)
    //  (packet ID 04) = 4 (header) + 2*NUM_CELLS   + 2 (checksum) + 1 (stop)
    static uint8_t packetbuff[4 + 2*25 + 2 + 1] = {0x0}; // buffer size suitable for up to 25 cells

    static uint32_t totalDataSize = 0;
    bool retVal = false;
    //hexDump(data,dataSize);

    if(totalDataSize + dataSize > sizeof(packetbuff)){
      Serial.printf("ERROR: datasize is overlength.");

      Serial.println(
        String("ERROR: datasize is overlength. ") +
        String("allocated=") +
        String(sizeof(packetbuff)) +
        String(", size=") +
        String(totalDataSize + dataSize)
        );

      totalDataSize = 0;
      packetstate = 0;

      retVal = false;
    }
    else if (data[0] == 0xdd && packetstate == 0) // probably got 1st half of packet
    {
        packetstate = 1;
        for (uint8_t i = 0; i < dataSize; i++)
        {
            packetbuff[i] = data[i];
        }
        totalDataSize = dataSize;
        retVal = true;

        if (data[dataSize - 1] == 0x77) {
            //its full packets
            packetstate = 2;
        }
    }
    else if (data[dataSize - 1] == 0x77 && packetstate == 1) //probably got 2nd half of the packet
    {
        packetstate = 2;
        for (uint8_t i = 0; i < dataSize; i++)
        {
            packetbuff[i + totalDataSize] = data[i];
        }
        totalDataSize += dataSize;
        retVal = true;
    }

    if (packetstate == 2) //got full packet
    {
        uint8_t packet[totalDataSize];
        memcpy(packet, packetbuff, totalDataSize);

        bmsProcessPacket(packet); //pass pointer to retrieved packet to processing function
        packetstate = 0;
        totalDataSize = 0;
        retVal = true;
    }
    return retVal;
}

bool bmsProcessPacket(byte *packet)
{

    bool isValid = isPacketValid(packet);

    if (isValid != true)
    {
        Serial.println("Invalid packer received");
        return false;
    }

    bmsPacketHeaderStruct *pHeader = (bmsPacketHeaderStruct *)packet;
    byte *data = packet + sizeof(bmsPacketHeaderStruct); // TODO Fix this ugly hack
    unsigned int dataLen = pHeader->dataLen;

    bool result = false;

    // find packet type (basic info or cell info)
    switch (pHeader->type)
    {
    case 3:
    {
        // Process basic info
        result = processBasicInfo(data, dataLen);
        break;
    }

    case 4:
    {
        // Process cell info
        result = processCellInfo(data, dataLen);
        break;
    }

    default:
        result = false;
        Serial.printf("Unsupported packet type detected. Type: %d", pHeader->type);
    }

    return result;
}


bool processBasicInfo(byte *data, unsigned int dataLen){
    //// NICER!!!: https://github.com/neilsheps/overkill-xiaoxiang-jbd-bms-ble-reader/blob/main/src/main.cpp

    uint16_t Volts = ((uint32_t)two_ints_into16(data[0], data[1])) * 10; // Resolution 10 mV -> convert to milivolts   eg 4895 > 48950mV
    int32_t Amps = ((int32_t)two_ints_into16(data[2], data[3])) * 10;   // Resolution 10 mA -> convert to miliamps

    int32_t Watts = Volts * Amps / 1000000; // W

    //Serial.printf("Remaining Capacity: %4.2fAh\n", ((float)(data[4] * 256 + data[5]))/100);
    //Serial.printf("Nominal Capacity: %4.2fAh\n", ((float)(data[6] * 256 + data[7]))/100);

    uint32_t CapacityRemainAh = ((uint16_t)two_ints_into16(data[4], data[5])) * 10;
    uint8_t CapacityRemainPercent = ((uint8_t)data[19]);

    uint16_t Temp1 = (((uint16_t)two_ints_into16(data[23], data[24])) - 2731);
    uint16_t Temp2 = (((uint16_t)two_ints_into16(data[25], data[26])) - 2731);

    uint16_t BalanceCodeLow = (two_ints_into16(data[12], data[13]));
    uint16_t BalanceCodeHigh = (two_ints_into16(data[14], data[15]));
    uint8_t MosfetStatus = ((byte)data[20]);

    eth_send("test.RC.%s.Voltage %f",currentName, (float)Volts / 1000);
    eth_send("test.RC.%s.Amps %f",currentName, (float)Amps / 1000);
    eth_send("test.RC.%s.Watts %f",currentName, (float)Watts);
    eth_send("test.RC.%s.Capacity_Remain_Ah %f",currentName, (float)CapacityRemainAh / 1000);
    eth_send("test.RC.%s.Capacity_Remain_Wh %f",currentName, ((float)(CapacityRemainAh) / 1000) * ((float)(Volts) / 1000));
    eth_send("test.RC.%s.Capacity_Remain_Percent %d",currentName, CapacityRemainPercent);
    eth_send("test.RC.%s.Temp1 %f",currentName, (float)Temp1 / 10);
    eth_send("test.RC.%s.Temp2 %f",currentName, (float)Temp2 / 10);
    /*
    Serial.printf("%s Balance Code Low: 0x%x\r\n",currentName, BalanceCodeLow);
    Serial.printf("%s Balance Code High: 0x%x\r\n",currentName, BalanceCodeHigh);
    Serial.printf("%s Mosfet Status: 0x%x\r\n",currentName, MosfetStatus);
    */
    gotBasicInfo=true;

    return true;
}

bool processCellInfo(byte *data, unsigned int dataLen)
{
    uint16_t _cellSum;
    uint16_t _cellMin = 5000;
    uint16_t _cellMax = 0;
    uint16_t _cellAvg;
    uint16_t _cellDiff;

    uint8_t NumOfCells = dataLen / 2; // data contains 2 bytes per cell

    //go trough individual cells
    for (byte i = 0; i < dataLen / 2; i++){
        int CellVolt = ((uint16_t)two_ints_into16(data[i * 2], data[i * 2 + 1])); // Resolution 1 mV
        _cellSum += CellVolt;
        if (CellVolt > _cellMax)
        {
            _cellMax = CellVolt;
        }
        if (CellVolt < _cellMin)
        {
            _cellMin = CellVolt;
        }

        eth_send("test.RC.%s.Cell.%d.Voltage %f",currentName, i+1,(float)CellVolt/1000);
    }

    eth_send("test.RC.%s.Max_Cell_Voltage %f",currentName, (float)_cellMax / 1000);
    eth_send("test.RC.%s.Min_Cell_Voltage %f",currentName, (float)_cellMin / 1000);
    eth_send("test.RC.%s.Difference_Cell_Voltage %f",currentName, (float)(_cellMax - _cellMin) / 1000);
    eth_send("test.RC.%s.Average_Cell_Voltage %f",currentName, (float)(_cellSum / NumOfCells) / 1000);

    gotCellInfo=true;

    return true;
}

bool isPacketValid(byte *packet) //check if packet is valid
{
    if (packet == nullptr){
        return false;
    }

    bmsPacketHeaderStruct *pHeader = (bmsPacketHeaderStruct *)packet;
    int checksumPos = pHeader->dataLen + 2; // status + data len + data

    int offset = 2; // header 0xDD and command type are not in data length

    if (packet[0] != 0xDD){
        // start bit missing
        return false;
    }

    if (packet[offset + checksumPos + 2] != 0x77){
        // stop bit missing
        return false;
    }

    byte checksum = 0;
    for (int i = 0; i < checksumPos; i++){
        checksum += packet[offset + i];
    }
    checksum = ((checksum ^ 0xFF) + 1) & 0xFF;

    if (checksum != packet[offset + checksumPos + 1]){
        return false;
    }

    return true;
}

int16_t two_ints_into16(int highbyte, int lowbyte) // turns two bytes into a single long integer
{
    int16_t result = (highbyte);
    result <<= 8;                //Left shift 8 bits,
    result = (result | lowbyte); //OR operation, merge the two
    return result;
}

/////

/*
   * ETH_CLOCK_GPIO0_IN   - default: external clock from crystal oscillator
   * ETH_CLOCK_GPIO0_OUT  - 50MHz clock from internal APLL output on GPIO0 - possibly an inverter is needed for LAN8720
   * ETH_CLOCK_GPIO16_OUT - 50MHz clock from internal APLL output on GPIO16 - possibly an inverter is needed for LAN8720
   * ETH_CLOCK_GPIO17_OUT - 50MHz clock from internal APLL inverted output on GPIO17 - tested with LAN8720
*/
#define ETH_CLK_MODE    ETH_CLOCK_GPIO0_IN  //  ETH_CLOCK_GPIO17_OUT

// Pin# of the enable signal for the external crystal oscillator (-1 to disable for internal APLL source)
#define ETH_POWER_PIN  16

// Type of the Ethernet PHY (LAN8720 or TLK110)
#define ETH_TYPE        ETH_PHY_LAN8720

// I²C-address of Ethernet PHY (0 or 1 for LAN8720, 31 for TLK110)
#define ETH_ADDR        1

// Pin# of the I²C clock signal for the Ethernet PHY
#define ETH_MDC_PIN     23

// Pin# of the I²C IO signal for the Ethernet PHY
#define ETH_MDIO_PIN    18

void WiFiEvent(WiFiEvent_t event) {
    Serial.print("E:");
    Serial.println(event);
  switch (event) {
    case ARDUINO_EVENT_ETH_START:
      Serial.println("ETH Started");
      //set eth hostname here
      ETH.setHostname("esp32-ethernet");
      break;
    case ARDUINO_EVENT_ETH_CONNECTED:
      Serial.println("ETH Connected");
      break;
    case ARDUINO_EVENT_ETH_GOT_IP:
      Serial.print("ETH MAC: ");
      Serial.print(ETH.macAddress());
      Serial.print(", IPv4: ");
      Serial.print(ETH.localIP());
      if (ETH.fullDuplex()) {
        Serial.print(", FULL_DUPLEX");
      }
      Serial.print(", ");
      Serial.print(ETH.linkSpeed());
      Serial.println("Mbps");
      eth_ready = true;
      break;
    case ARDUINO_EVENT_ETH_DISCONNECTED:
      Serial.println("ETH Disconnected");
      eth_ready = false;
      break;
    case ARDUINO_EVENT_ETH_STOP:
      Serial.println("ETH Stopped");
      eth_ready = false;
      break;
    default:
      break;
  }
}

// Overloaded template function for formatted sending
template<typename... Args>
void eth_send(const char* format, Args... args) {
    constexpr std::size_t bufferSize = 1024; // Define a reasonable default buffer size
    std::array<char, bufferSize> buffer;
    snprintf(buffer.data(), buffer.size(), format, args...);
    eth_send(buffer.data()); // Call the original eth_send function
}

void eth_send(const char *data) {
    if(!https.connected()){
        https.end();
        client_connect();
    }
  Serial.print("http connecting...\r\n");
  Serial.printf("ETH Send: %s\r\n", data);
  int httpCode=https.POST(data);
  if(httpCode==200){
      Serial.println("post ok");
      while (client.available()) {
        Serial.write(client.read());
      }
  }else{
      Serial.print("post fail:");
      Serial.println(httpCode);
  }
}

void eth_begin() {
  WiFi.onEvent(WiFiEvent);
  ETH.begin(ETH_ADDR, ETH_POWER_PIN, ETH_MDC_PIN, ETH_MDIO_PIN, ETH_TYPE, ETH_CLK_MODE);
}